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Abstract 

This  Paper  explores  the  use  of  an  extended  Kalman  filter  to  provide  real-time  esti- 
mates of  underwater  vehicle  position  and  attitude.  The  types  of  previously  available  sen- 
sors are  detailed  including  strapdovvn  accelerometers,  roll  and  pitch  sensors,  gyro  and 
magnetic  compasses,  depth  sensor,  and  various  types  of  acoustic  positioning  systems.  A 
doppler  velocimeter  is  added  to  this  sensor  suite  to  improve  the  perfonnance  of  the  filter. 
As  an  integral  part  of  the  filter,  magnetic  compass  and  gyrocompass  biases  are  estimated 
to  improve  vehicle  heading  accuracy.  The  filter  is  designed  to  account  for  numerous  real- 
life  complications.  These  include  varying  rates  of  sensor  output,  lengthy  gaps  in  reception 
of  position  information,  presence  of  non-Gaussian  position  fix  eiTors  (flyers),  and  varying 
probability  density  functions  for  sensor  errors.  Simulated  data  are  used  to  test  the  filter 
with  varying  availability  of  data  and  accuracy  of  initial  conditions,  along  with  actual  data 
from  a  deployment  of  the  towed  DSL- 120  vehicle.  The  increased  accuracy  obtained  by 
using  the  doppler  velocimeter  is  emphasized. 
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Chapter  1 

Introduction 


1.1  Motivation 

This  thesis  has  two  major  goals.  The  first  is  to  provide  accurate  estimates  of 
underwater  vehicle  position  and  attitude  and  thereby  improve  the  quality  of  sonar  and 
video  information  collected.  The  second  is  to  provide  these  estimates  in  real-time. 

1.1.1  Improving  Vehicle  Position  and  Attitude  Information 

More  than  two  thirds  of  the  Earth  is  covered  by  water.  Until  very  recently  in 
history,  the  ocean  depths  were  mysterious  and  inaccessible.  However,  in  the  past  few 
decades  new  technologies  have  dramatically  expanded  man's  abilities  to  explore  the  ocean 
bottom.  Discoveries  range  from  the  remnants  of  human  incursions,  epitomized  by  the 
Titanic,  to  new  life  on  the  ocean  floor  gaining  its  sustenance  from  the  earth's  interior 
rather  than  the  sun. 

One  of  the  primary  tools  for  conducting  underwater  searches  and  surveys  from 
unmanned  vehicles  is  the  sidescan  sonar.  The  sonar  emits  a  beam  of  relatively  high- 
frequency  sound  on  either  side  of  the  vehicle.  When  this  sound  beam  hits  the  bottom  or 
any  objects  within  range,  it  is  reflected  and  the  return  is  received  by  transducers  mounted 
on  the  vehicle.  The  characteristics  of  the  received  .sound,  primarily  magnitude  and  phase, 
can  then  be  analyzed  to  determine  the  characteristics  and  contours  of  the  bottom  as  well  as 
the  location  and  general  shape  of  any  objects. 

For  this  system  to  work  at  an  optimum  level,  it  is  vital  that  the  location  and  attitude 
of  the  vehicle  be  known  as  accurately  as  possible.  Any  position  errors  lead  directly  and 
obviously  to  an  error  in  the  assumed  position  of  all  objects  mapped  by  the  sonar.  Equally 
important,  however,  is  knowledge  of  the  vehicle's  roll,  pitch,  and  heading.  Due  to  the 
nature  of  sound  propagation  in  water,  a  significant  time  lag  occurs  between  the 
transmission  of  a  sonar  ping  and  the  receipt  of  any  returns.  During  this  time,  the  vehicle 
will  not  only  have  changed  position  but  may  have  changed  its  attitude  as  well.  For  proper 
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interpretation  of  the  sonar  returns,  the  attitude  of  the  vehicle  at  the  time  of  sonar 
transmission  must  be  known,  because  that  determines  the  precise  direction  of  the  beam. 
This  same  information  is  also  needed  at  the  time  of  the  sonar  return,  so  the  location  of  any 
objects  found  can  be  accurately  determined. 

1.1.2  Previous  Work  on  Underwater  Vehicle  Navigation 

The  invention  of  feasible  inertial  navigation  produced  a  great  interest  in  combining 
inertial  measurements  with  other  fix  sources  to  improve  navigational  accuracy.  These 
early  inertial  navigation  units  required  gimbal-stabilized  platforms  to  achieve  the  desired 
accuracy,  which  added  size  and  weight.  Due  to  these  restrictions,  early  work  was  confined 
to  aircraft  and  ships,  including  submarines.  In  the  early  1970s,  the  U.S.  Air  Force  was  one 
of  the  first  to  evaluate  the  use  of  the  Kalman  filter  to  integrate  inertial  measurements  with 
external  fix  sources  (D'Appolito,  1971). 

Later,  strapdown  inertial  systems  permitted  much  smaller  and  lighter  equipment, 
though  at  a  cost  in  accuracy.  Once  again,  the  military  provided  a  major  impetus  to 
developing  these  .systems  to  their  fullest  potential.  For  example,  the  North  Atlantic  Treaty 
Organization's  (NATO)  Advisory  Group  for  Aerospace  Research  and  Development 
(AGARD)  included  much  work  on  the.se  strapdown  .systems,  although  still  for  use 
primarily  in  aircraft  and  spacecraft  {VanBronkhorst,  1978;  Catford,  1978). 

Ever  since  remotely  operated  vehicles  (ROVs)  became  feasible,  there  has  been 
interest  in  improving  their  navigational  accuracy  as  well.  Early  ROVs  had  only  a  few 
sensors  available  to  provide  navigational  infonnation,  since  inertial  navigational  systems 
were  still  too  heavy  and  power-intensive  to  be  usable.  The.se  included  a  magnetic  compass 
to  provide  heading  and  inclinometers  to  measure  pitch  and  roll.  Additionally,  an  acoustic 
transponder  network  was  often  used  to  provide  position  information  for  operations  in  a 
small  area.  A  later  improvement  was  the  gyrocompass,  which  provides  a  more  consistent 
output  than  the  magnetic  compass  but  adds  new  errors  caused  by  drift. 

Improvements  in  the  information  provided  by  these  acoustic  transponder  nets  has 
been  marked.  In  her  1992  thesis,  Diane  Di  Massa  provided  one  example  with  her 
exploration  of  the  possibilities  of  hyperbolic  navigation  to  permit  long-range  acoustic 
navigation  by  an  Autonomous  Underwater  Vehicle  (AUV)  {Di  Massa,  1992).  Brian 
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Tracey  contributed  another  with  his  improvements  to  a  newer  technique  for  ROV 
navigation,  ultrashort-baseline  navigation,  in  his  1992  thesis  (Tracey,  1992). 

In  the  past  few  years,  a  number  of  new  devices  have  been  developed  to  assist  in 
solving  this  navigational  problem.  In  his  1988  thesis,  Gregory  Vaughn  discussed  the  use  of 
a  lightweight,  strapdown  inertial  motion  unit  in  conjunction  with  an  external  acoustic 
positioning  net  to  improve  closed-loop  vehicle  conu-ol  (Vaughn,  1988).  He  concluded  that 
by  incorporating  this  new  data  into  a  Kalman  filter  to  provide  real-time  optimal  estimates 
of  vehicle  parameters,  closed-loop  control  of  ROVs,  such  as  the  Woods  Hole 
Oceanogi'aphic  Institution's /a^o/j,  could  be  improved.  However,  for  his  application 
vehicle  attitude  was  unimportant  and  therefore  was  not  incorporated  into  his  simulations. 

A  fairly  recent  addition  to  the  repertoire  of  instruments  available  to  the 
oceanographic  engineer  is  the  doppler  velocimeter,  also  known  as  the  acoustic  doppler 
current  profiler  (ADCP).  For  the  first  time,  accurate  real-time  velocity  measurements  of  a 
ROV  could  be  made  relatively  cheaply  and  without  exceeding  the  restrictive  power  and 
weight  limits  inherent  in  ROV  operations.  RD  Instruments,  a  doppler  velocimeter 
manufacturer,  has  been  funded  by  the  Office  of  Naval  Research  to  explore  the  use  of  the 
instrument  to  provide  an  independent  and  accurate  measurement  of  vehicle  velocity 
within  the  context  of  a  Kalman  filter.  Their  initial  work  verifies  the  utility  of  the 
velocimeter,  although  so  far  their  research  has  been  confined  to  inertial  navigation  systems 
too  large  for  ROV  u.se  (Rowc  and  Brumlcy,  1992). 

These  ai'e  but  a  few  examples  of  the  extensive  work  that  has  been  published  on 
improving  underwater  vehicle  navigation.  A  search  of  the  literature  reveals  diverse 
techniques  for  improving  sensor  infomiation  and  integration,  including  several  variations 
on  Kalman  filtering  techniques. 

1.1.3  Cost  and  Processing  Time  Reduction 

In  the  real  world,  cost  considerations  are  almost  always  important.  Cruises  are 
expensive  and  usually  result  in  computer  disks  full  of  unproce.ssed,  raw  data,  which  are 
essentially  worthless  until  processed.  This  processing  can  take  many  man  hours,  which 
translates  into  a  considerable  cost.  Unfortunately,  this  sometimes  means  that  processing  is 
never  completed,  since  either  the  time  or  the  money  runs  out.  Therefore,  the  more  real- 
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time  processing  that  can  be  performed  the  better.  Real-time  processing  means  that  the 
important  data  are  immediately  available.  Also,  since  it  is  accomplished  at  sea  while  the 
cruise  is  in  progress  (and  already  paid  for),  additional  expenses  are  minimized. 

Another  important  application  for  this  capability  pertains  to  autonomous 
underwater  vehicles  (AUVs).  Much  research  is  being  conducted  currently  on  these 
vehicles,  which  should  be  capable  of  operating  independently  for  days  or  even  weeks. 
Real-time  processing  is  essential  to  permit  the  vehicle  to  navigate  successfully  and  is  also 
vital  in  reducing  the  on-board  data  storage  requirements  of  AUV  deployments. 

1.1.4  Resources  Available 

This  thesis  investigates  how  tiie  navigational  sensors  currently  available  at  the 
Deep  Submergence  Laboratory  (DSL)  can  be  used  to  improve  the  real-time  position  and 
attitude  information  available  for  the  operation  of  a  ROV.  This  summer,  DSL  personnel 
pai'ticipatcd  in  a  cruise  aboard  the  RA^  Kiiorr.  A  major  purpose  of  the  cruise  was  to  obtain 
detailed  sidescan  sonai"  data  from  an  area  of  the  Mid- Atlantic  Ridge  where  seafloor 
spreading  is  occuriing.  The  DSL- 120,  a  lowed  vehicle  cairying  a  120-kHz  sidescan  sonai" 
system  and  multiple  navigational  sensors,  was  used  for  this  sui"vey.  Data  from  these  DSL- 
120  deployments  are  used  to  demonstrate  the  perfonnance  of  the  Kalman  filter  developed 
in  this  thesis.  By  integrating  the  velocity  data  provided  by  the  doppler  velocimeter  into  a 
Kalman  filter  algorithm,  the  vehicle's  navigational  accuracy  can  be  gready  enhanced.  This 
substantially  improves  the  usefulness  of  the  sonar  data  collected  and  demonstrates  the 
feasibility  of  performing  real-time  navigation  sensor  processing. 


1.2  Advantages  and  Limitations  of  the  Kalman  Filter 

Sonar  data  collected  by  underwater  vehicles  has  often  been  rendered  nearly  u.seless 
due  to  inaccurate  navigation  information  and  an  overly  simplistic  approach  to  tracking 
vehicle  parameters.  For  example,  relying  solely  on  a  magnetic  compa.ss  for  heading 
information  nearly  guarantees  that  the  measured  heading  will  be  inaccurate. 

The  Kalman  filter  uses  a  state  space  model  to  provide  a  well  defined  method  for 
integrating  the  information  obtained  from  different  sensors  into  a  coherent  whole.  Taking 
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advantage  of  the  a  priori  knowledge  of  the  relative  accuracies  of  these  sensors  as  well  as 
the  dynamics  of  the  system,  the  Kalman  filter  can  provide  real-time  output  without 
overstressing  computer  memory  or  computational  limits. 

The  addition  of  the  doppler  velocimeter  provides  a  marked  improvement  in  the 
ability  of  a  Kalman  filter  to  provide  valid  position  and  attitude  information.  By  providing 
accurate  ineasurements  of  the  magnitude  of  vehicle  velocities  in  all  three  dimensions,  a 
more  precise,  condnuous  esdmate  of  vehicle  position  and  attitude  can  be  provided  than 
would  be  available  by  the  other  sensors  alone. 

In  this  application,  there  are  several  obstacles  to  overcome  in  creating  a  usable 
Kalman  filter.  Fir.st,  the  system  is  nonlinear  and  cannot  therefore  use  the  simplest  and  most 
mathematically  precise  version  of  Kalman  filter  theoi^.  Instead,  an  extended  Kalman  filter 
is  required.  Second,  a  method  for  ignoring  obviously  faulty  measurements  must  be 
included.  Finally,  there  ai-e  several  practical  problems  involved  in  using  the  output  of  such 
a  wide  variety  of  sen.sors.  These  problems  and  their  proposed  solutions  are  di.scussed  in 
detail  later  in  this  thesis. 


1.3  Thesis  Outline 

Chapter  Two  de.scribes  the  sensors  used  to  provide  infonnation  for  the  Kalman 
filter.  Principles  of  operation  for  each  sensor  are  discussed,  as  well  as  limitations  and 
considerarions  for  each  when  integrated  into  the  overall  system. 

Chapter  Three  explains  the  development  of  the  Kalman  filter  and  its  state  space 
model  for  this  application.  A  basic  review  of  Kalman  filter  theory  is  included,  with 
emphasis  on  the  specifics  of  the  extended  Kalman  filter  required  for  nonlinear  problems. 
This  chapter  also  discusses  the  specific  problems  that  were  addres.sed  for  proper  filter 
operadon  and  how  the  filter  was  implemented. 

Chapter  Four  shows  the  results  of  filter  operation  by  using  both  simulated  and 
actual  data.  The  improvement  in  performance  obtained  by  using  the  doppler  velocimeter 
measurements  of  vehicle  velocities  is  emphasized. 


14 


The  results  are  summarized  in  Chapter  Five,  along  with  suggestions  for  further 
refinements  to  the  filter.  Finally,  some  possibilities  for  additional  experiments  are 
discussed. 
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Chapter  2 

Sensor  Information 


2.1  Overview  of  Sensors 

Like  any  mathematical  abstraction  of  a  real-world  system,  the  Kalman  filter  is  a 
simplified  model  of  reality.  For  the  filter  to  function  optimally,  the  model  must  be  as 
complete  and  as  accurate  as  possible.  For  this  application  there  are  two  major  aspects  of 
the  filter  tliat  must  be  considered.  The  first  concerns  prediction:  based  on  knowledge  of 
present  system  parameters,  what  will  happen  to  these  parameters  in  the  future?  The  second 
is  more  basic:  how  can  knowledge  of  these  system  parameters  be  obtained? 

In  this  chapter,  the  latter  issue  is  addressed.  Each  sensor  providing  input  to  the 
filter  is  described  in  detail.  Using  our  knowledge  of  the  available  sensors,  the  Kalman 
filter  model  can  then  be  developed.  Before  proceeding  further,  however,  a  clarification  of 
the  coordinate  systems  used  is  required. 


2.2  Coordinate  Systems 

Within  the  filter  algorithm,  two  different  coordinate  systems  are  used.  The  first  is 
earth-referenced,  with  positions  measured  relative  to  fixed  reference  points  on  the  earth. 
The  second  is  vehicle-referenced,  also  referred  to  as  body-referenced.  In  this  case,  all 
measurements  are  made  relative  to  a  specific  point  on  the  ROV,  usually  the  center  of 
rotation  of  the  vehicle.  Table  2-1  and  Figure  2-1  explain  the  differences  in  more  detail. 

An  additional  coordinate  system  is  .sensor-referenced,  which  depends  on  the 
location  of  the  individual  sensor.  For  linear  acceleration  and  velocity  measurements,  a 
difference  in  measurement  magnitudes  between  sensor-referenced  and  vehicle-referenced 
measurements  exi.sts  due  to  the  effects  of  angular  velocities  when  the  sensor  is  mounted 
away  from  the  vehicle's  center  of  rotation.  The  orientation  of  the  axes  is  the  same  as 
vehicle-referenced  coordinates  since  the  instruments  are  mounted  colinear  with  the 
vehicle  axes.  Compensation  for  this  difference  is  di.scus.sed  in  Section  3.5.3. 


16 


Up 

k  +Z  Direction 

Side 

fj    ROV  Starboard  Side      \ 

Forward 

View 

^ 

+Y  Direction 

Top 

I  )          ROV 

Top  Side          \ 

Forward 

View 

v/ 

^                  J^ 

^ 

yj  To  Starboard 
+X  Direction 

+Y  Direction 

Figure  2-1.  Vehicle-referenced  coordinate  system. 


Table  2-1:  Comparison  of  Coordinate  Systems 


Direction 

Earth-Referenced 

Vehicle-Referenced 

X 

Longitude  (-i-X  =  east) 

Peipendicuiar  to  side  of 
vehicle  (+X  =  to  starboard) 

Y 

Latitude  (+Y  =  noilh) 

In  direction  of  vehicle  bow 
or  stern  (+Y  =  forward) 

Z 

Deptii/Altitude  (+Z  =  up) 

Pei"pendicular  to  top  or  bot- 
tom of  vehicle  (+Z  =  up) 

2.3  Doppler  Velocimeter 

The  proper  name  for  this  instrument,  used  to  provide  vehicle  velocities,  is  the 
Direct-Reading  Broadband  Acoustic  Doppler  Current  Profiler  (DR-BBADCP). 
Manufactured  by  RD  Instruments  (RDI),  the  DR-BBADCP  is  designed  to  measure  current 
velocities  at  di.screte  points  through  the  water  column  (see  Fig.  2-2).  Its  alternate  mode, 
which  is  the  mode  used  to  gather  data  for  this  application,  is  the  bottom-track  profiling 
mode.  With  the  DR-BBADCP  mounted  on  the  vehicle  looking  downward,  vehicle 
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Figure  2-2.  Broadband  ADCP  setup  for  current  profiling  {RDI,  1993). 

instrument  in  this  application,  the  more  descriptive  name  of  "doppler  velocimeter"  is  used 
in  the  remainder  of  this  thesis. 

2.3.1  Principles  of  Operation 

The  doppler  velocimeter  uses  four  downward-looking  acoustic  beams  operating  at 
high  frequency,  figure  2-3  shows  the  beam  pattern  of  two  of  these  beams  in  an  upward- 
looking  mode.  RDI  manufactures  these  instruments  with  six  different  transmit 
frequencies:  75,  150,  300,  600,  1200,  and  2400  kHz.  As  with  any  sonar,  increasing 
frequency  improves  the  accuracy  but  reduces  the  effective  range.  Therefore,  the  frequency 
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Fig.  2-3.  ADCP  beam  geometry  {RDI,  1993). 

for  a  given  operation  is  chosen  to  be  as  high  as  possible  while  still  providing  sufficient 
range  to  ensonify  the  bottom  based  on  the  expected  altitude  of  the  vehicle. 

As  evidenced  by  its  name,  the  instrument  operates  on  the  doppler  principle. 
Vehicle  velocity  in  the  direction  of  a  beam  increases  the  frequency  of  the  returned  signal, 
while  velocity  away  from  the  beam  decreases  it.  Specifically,  the  vehicle  velocity  in  the 
direction  of  the  beam  is  calculated  by  {RDI,  1993). 

c 
Relative  Flow  Velocity  (m/s)  =  /V)  x  — —    , 


where 


Fr-j  is  the  measured  doppler  frequency  shift  in  kHz, 

c  is  the  speed  of  sound  in  water  at  the  transducer  face  in  m/s, 

Ff.  is  the  transmitted  acoustic  frequency  in  kHz. 
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Fig.  2-4.  Frequency  change  caused  by  doppler  effect  (RDI,  1993). 

Figure  2-4  shows  this  effect  with  a  fixed  transducer  and  moving  material  in  the 
water.  The  effect  is  the  same  with  the  iran.sducer  mounted  on  a  moving  vehicle  over  the 
fixed  bottom.  The  four  beams  transmit  at  different  angles  so  the  velocities  in  each 
direction  can  be  resolved.  Three  are  needed  to  solve  for  velocities  in  all  three  directions 
and  the  fourth  provides  redundancy  to  ensure  accuracy. 

To  interpret  the  received  sonar  data,  the  doppler  velocimeter  uses  an 
autocorrelation  processor.  A  series  of  shon  pulses  is  transmitted,  each  with  a  pulse  length 
of  Tp,  with  a  known  lag  Tl  between  pulses.  When  the  returns  are  received,  the  processor 
compares  the  phase  change  between  two  distinct  pulses,  accounting  for  the  difference  in 
time  between  their  respective  transmissions.  As  shown  in  Fig.  2-5,  a  zero  phase  change 
implies  zero  velocity.  Likewi.se,  a  phase  change  equates  to  a  velocity  with  magnitude 
determined  by  the  amount  of  phase  change.  Phase  changes  with  a  magnitude  greater  than 
27t  are  resolved  using  a  proprietary  RDI  algorithm  using  different  subsets  of  the  *• 
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Fig.  2-5.  Phase  change  seen  by  the  broadband  system  {RDI,  1993). 

transmitted  pulses.  If  any  datum  is  below  the  operator-selector  minimum  for  correlation 
magnitude,  that  clatum  is  rejected  by  the  doppler  velocimeter's  processor. 

The  output  of  the  velocimeter  can  be  referenced  four  different  ways.  The  most 
basic  provides  measurements  of  the  velocities  relative  to  each  of  the  four  beams. 
Alternatively,  the  velocimeter's  internal  proces.sor  can  re.solve  the  components  of  the  four 
beams  to  provide  velocity  fore  or  aft,  velocity  left  or  right,  and  velocity  up  or  down 
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relative  to  the  instrument.  Third,  if  the  instrument  is  not  mounted  on  a  ship  or  ROV  with 
the  same  orientation  as  the  platform's  reference  axes,  the  velocities  can  be  converted  to 
any  desired  direction  relative  to  the  platform  using  knowledge  of  the  difference  between 
the  instrument  axes  and  platfonn  axes.  Finally,  the  doppler  velocimeter  can  use  its  internal 
flux-gate  sensor,  which  uses  heading  and  pendulum  sensors  to  provide  pitch  and  roll.  The 
internal  processor  can  use  this  roll,  pitch,  and  heading  information  or  similar  data  from 
external  sensors  to  convert  from  vehicle-referenced  to  earth-referenced  velocities. 

For  this  Kalman  filter  the  velocimeter  is  mounted  with  axes  oriented  the  same  as 
the  vehicle  axes  described  in  Section  2.2.  Therefore,  a  coordinate  transformation  to  the 
platform  axes  is  unnecessary  except  for  any  coiTection  due  to  angular  velocities.  Instead  of 
allowing  the  velocimeter's  internal  processor  to  convert  velocities  to  earth-referenced 
coordinates,  which  would  use  the  cuiTent  output  of  the  appropriate  sensor  for  attitude 
measurements,  the  filter  uses  the  vehicle-referenced  values.  Using  vehicle-referenced 
velocities  allows  the  filtered  estimates  of  roll,  pitch,  and  heading  to  be  u.sed  to  make  the 
conversion  to  earth-referenced  coordinates.  This  improves  overall  filter  perfonnance  since 
the  filtered  estimates  are  less  noisy  than  the  instantaneous  sensor  outputs. 

2.3.2  Inputs  to  Velocimeter 

To  provide  the  desired  output,  the  doppler  velocimeter  uses  the  speed  of  sound  in 
its  calculations  of  vehicle  velocity.  As  is  the  case  for  data  used  in  this  thesis,  this  speed  of 
sound  is  typically  entered  manually  by  the  operator  to  pennit  easier  post-processing  of  the 
data.  However,  for  more  accurate  calculations  the  doppler  velocimeter  can  calculate  the 
current  speed  of  sound  using  three  inputs:  salinity,  depth,  and  temperature. 

For  most  open-ocean  operations  salinity  is  constant.  Therefore,  salinity  can  be 
manually  inserted  prior  to  operations.  If  operations  are  to  be  conducted  in  an  area  of 
varying  salinity,  such  as  near  a  river  mouth,  then  an  external  conductivity  sensor  can  be 
used  to  provide  salinity  information  to  the  velocimeter. 

Since  the  majority  of  vehicle  operations  occur  within  a  relatively  narrow  depth 
band,  making  speed  of  sound  changes  due  to  depth  change  negligible,  depth  is  also 
normally  manually  inserted.  If  operations  involving  large  depth  changes  are  expected. 
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consideration  should  be  given  to  providing  deptii  estimates  to  the  velocimeter  to  calculate 
a  variable  speed  of  sound. 

Finally,  the  doppler  velocimeter  has  an  internal  temperature  sensor.  Manual  input 
can  be  used  in  this  case  as  well.  For  operations  near  the  bottom  in  deep  water,  where 
thermal  gradients  are  negligible,  manually  inputting  temperature  pennits  more  precise 
post-processing  of  the  data  without  introducing  significant  errors.  In  areas  with  larger 
gradients,  using  the  installed  temperature  sensor  is  necessary. 

2.3.3  Doppler  Velocimeter  Outputs 

Obviously,  the  primary  output  desired  from  the  doppler  velocimeter  is  vehicle- 
referenced  velocity.  However,  there  are  several  operator-selected  options  that  affect  the 
accuracy  and  the  frequency  of  these  measurements. 

Frequency  of  output  is  determined  by  three  factors:  the  speed  of  sound  in  the 
water,  the  range  to  the  bottom  (vehicle  altitude),  and  the  number  of  pings  per  ensemble. 
The  doppler  velocimeter  averages  the  results  from  the  pings  in  each  ensemble  to  improve 
accuracy.  The  technical  manual  recommends  four  pings  per  ensemble.  A  larger  number  of 
pings  per  ensemble  results  in  better  quality  measurements  but  at  the  cost  of  a  lower  update 
rate.  Using  the  recommended  four  pings  per  ensemble,  an  average  altitude  of  100m,  and 
the  nominal  speed  of  sound  in  seawater  of  1500  m/s,  the  time  between  velocity 
measurements  can  be  calculated  as 

4  pings  X  200  in/ping  x  1  s/1500  m  =  0.53  s/ensemble. 

The  standard  deviation  for  velocity  measurements  is  computed  by  the 
velocimeter's  processor  ba.sed  on  the  BBADCP  frequency  (for  the  model  in  use),  the 
range  to  the  bottom,  the  number  of  pings  per  en.semble,  and  the  size  of  the  depth  cells 
selected  prior  to  operation.  The  size  of  the  depth  cell  is  imponant  for  precision  when 
operating  in  the  water  column  profiling  mode.  For  bottom  tracking,  however,  the  largest 
depth  cell  should  always  be  used  since  this  gives  the  lowest  standard  deviation  for  vehicle 
velocity  measurements.  For  vehicle  operations  treated  in  this  thesis,  the  standard  deviation 
is  on  the  order  of  1-2  cm/s.  Figure  2-6  shows  how  accuracy  varies  with  range  for  a  single 
ping  using  the  150-kHz  model.  This  high  accuracy  enables  precise  measurement  of 
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Fig.  2-6.  Standard  deviation  for  a  single  ping  using  the  150-kHz  system  (with  ADCP 
velocity,  from  top  to  bottom,  of  20  knots,  10  knots,  and  0  knots)  (/?D/,  1993). 


velocity  magnitude.  However,  overall  system  accuracy  is  limited  by  how  well  heading, 
pitch,  and  roll  can  be  estimated.  Since  standard  deviation  is  constantly  updated  by  the 
doppler  velocimeter's  software,  that  information  can  be  included  in  the  Kalman  filter 
algorithm.  Specifics  of  this  implementation  are  discussed  in  Chapter  3. 


2.4  Depth  Sensor 

The  depth  sensor  used  for  this  work  is  manufactured  by  Paroscientific,  Inc.  and 
consists  of  a  quartz-crystal  resonator  whose  frequency  of  oscillation  varies  with  pressure- 
induced  stress.  The  sensor  also  includes  thennal  compensation  using  a  quanz-crystal 
temperature  signal.  Nominal  accuracy  of  the  sensor  is  0.02%,  and  a  reading  can  be 
obtained  approximately  every  0.25s.  {Paroscientific,  1987). 

For  the  piirpo.ses  of  achieving  the  best  overall  estimates  of  vehicle  position  and 
velocity,  the  ab.solute  eiTor  of  this  .sensor  is  less  important  than  its  stability.  Previous 
experience  indicates  that  the  depth  sensor  should  be  very  consistent  in  its  outputs. 
Therefore,  the  output  of  the  .sensor  is  a  vital  element  in  increasing  the  accuracy  of  the 
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Kalman  filter  used  to  detemiine  state  space  estimates  for  depth-dependent  vehicle 
parameters. 

The  only  time  the  sensor  should  exhibit  less  stability  than  desired  is  if  the  sensor  is 
not  in  thennal  equilibrium  with  the  surrounding  water.  This  could  occur  soon  after  initial 
deployment.  However,  in  this  case  the  problem  would  be  foreseen,  and  thermal 
equilibrium  could  be  achieved  prior  to  commencing  actual  search  operations.  Of  more 
concern  is  operation  in  water  with  strong  thermal  gradients.  This  problem  could  be 
particularly  acute  in  shallow-water  operations,  especially  at  certain  times  of  the  day. 
During  such  operations,  the  pressure  sensor  can  be  expected  to  give  less  accurate  results, 
resulting  in  greater  Kalman  filter  en-ors. 


2.5  External  Positioning  System 

There  ai'e  several  possible  systems  that  can  be  used  to  provide  external  position 
infonnation.  Historically,  nearly  all  underwater  vehicle  positioning  information  has  been 
obtained  from  some  type  of  acoustic  net.  These  acoustic  systems  generally  fall  into  three 
different  categories,  with  the  type  used  for  a  specific  operation  dependent  on  the  type  of 
mission  and  the  topography  of  the  bottom. 

2.5.1  Long-Baseline  Systems 

In  a  long-baseline  system,  an  array  of  acoustic  transponders  is  deployed  on  the 
bottom  in  the  vicinity  of  projected  vehicle  operations.  A  transponder  is  also  attached  to  the 
vehicle.  Vehicle  position  is  detemiined  by  measuring  the  travel  times  of  sound  waves 
between  the  vehicle  and  the  transponders  in  the  net.  For  two-way  systems,  the  vehicle 
sends  out  a  ping,  and  each  transponder  re.sponds  with  a  coded  return  signal  when  it 
receives  the  initial  ping  from  the  vehicle.  The  travel  times  are  then  translated  into  range. 
One-way  systems  use  the  same  principle,  but  travel  time  is  measured  only  from  the 
vehicle  to  each  transponder  in  the  net.  With  either  system,  since  the  positions  of  the 
transponders  on  the  bottom  have  already  been  determined  when  calibrating  the  acoustic 
net,  the  position  of  the  vehicle  can  be  determined  {Mon^an,  1978). 
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An  example  of  such  a  system  is  the  Sonic  High  Accuracy  Ranging  and  Positioning 
System  (SHARPS).  This  commercial  system  uses  three  transceivers  near  the  ocean 
bottom,  which  are  connected  to  a  surface  ship  by  coaxial  cables.  Another  transceiver  is 
mounted  on  the  ROV,  which  is  also  connected  to  the  ship.  Because  the  cable  connections 
allow  the  surface  ship  to  know  the  exact  transmission  time  of  each  ping,  only  one-way 
travel  times  are  needed,  which  allows  twice  the  frequency  of  updates  as  a  two-way 
system.  Operating  at  300  kHz,  SHARPS  is  limited  to  a  range  of  100  m  in  seawater  but  can 
provide  accuracies  on  the  order  of  2  cm  {Somers,  1991). 

A  disadvantage  of  SHARPS  is  the  need  for  multiple  cables  from  the  surface  ship  to 
the  transponder  field,  which  can  cause  difficulties  when  maneuvering  a  ROV  in  the  area. 
Another  system,  EXACT,  was  used  by  Yoerger  and  Mindell  to  control  the  ROV  Jason  in 
the  vicinity  of  an  active  hydrothermal  plume  at  a  depth  of  2200  meters.  The  EXACT 
system  operates  similarly  to  SHARPS  and  provides  comparable  accuracies  but  uses  no 
cable  connections  to  the  surface.  Instead,  the  master  unit  on  the  ROV  communicates  with 
the  surface  through  a  low-bandwidth  serial  link.  Because  there  are  no  cable  connections, 
two-way  travel  times  are  used  {Yoerger  and  Mindell,  1992). 

The  major  advantages  of  a  long-baseline  system  are  its  stability  and  its  accuracy 
when  conducting  vehicle  operations  in  deep  water  close  to  the  bottom.  Because  the 
ti-ansponder  array  is  on  the  seafloor,  it  is  fixed  once  placed  and  calibrated.  Therefore,  its 
accuracy  is  improved  over  a  less  stable  system.  Since  the  an-ay  of  transponders  is 
deployed  on  the  bottom  near  the  area  of  operations,  the  difference  in  acoustic  travel  times 
between  each  pinger  and  the  vehicle  will  be  greater  than  if  all  pingers  were  located  close 
to  the  surface,  which  also  enhances  accuracy.  Finally,  the  acoustic  net  can  be  made  as 
large  as  desired.  By  using  lower  frequency  systems,  the  maximum  range  to  the  ROV  can 
be  greatly  increased  at  the  cost  of  reduced  accuracy  and  decreased  update  frequency. 

There  are  two  major  disadvantages  of  this  method.  First,  because  a  transponder 
net  must  be  deployed  on  the  bottom  and  calibrated,  significant  preparation  time  is  required 
before  ROV  operations  can  begin.  Second,  vehicle  operations  are  restricted  to  the  vicinity 
of  the  acoustic  transponders.  Therefore,  for  large-area  surveys,  this  method  is  impractical. 
In  this  case,  a  short-baseline  system  may  be  used  (Morgan,  1978). 
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2.5.2  Short-Baseline  Systems 

The  principles  of  operation  of  the  short-baseline  systems  are  similar  to  those 
having  a  long  baseline.  In  fact,  a  system  such  as  SHARPS  can  be  used  in  either 
configuration.  The  obvious  difference  is  that  instead  of  the  acoustic  net  transponders  being 
placed  on  the  ocean  floor,  they  are  attached  to  the  surface  ship.  This  method  eliminates 
some  disadvantages  of  the  long-baseline  system.  Since  the  transponders  are  mounted  on 
the  surface  vessel  in  known  locations,  no  initial  surveying  is  required.  Also,  the  ship  can 
be  moved  wherever  operations  are  desired,  so  the  range  limitations  of  a  bottom-fixed  net 
are  negated. 

Like  the  long-baseline  system,  the  short-baseline  system  measures  the  difference 
in  arrival  time  from  the  acoustic  pinger  on  the  ROV  to  each  transponder  on  the  ship. 
However,  becau.se  the  geometry  of  the  ship-mounted  aiTay  is  precisely  known,  this 
difference  in  amval  times  can  be  converted  into  an  angular  direction  from  the  plane  of  the 
array  to  the  ROV.  Since  the  ROV  is  tethered  to  the  ship,  the  time  of  each  ping  is  also 
known  precisely.  Therefore,  both  bearing  and  range  information  are  available,  yielding  the 
location  of  the  ROV  {Morgan,  1978). 

However,  the  short-baseline  system  tends  to  increase  position  errors  because  of  the 
more  sensitive  geometry.  Also,  because  the  transponder  array  is  now  mounted  on  a 
moving  platform,  surface  positioning  errors  can  become  the  dominant  factor  in 
determining  vehicle  location.  The  importance  of  accurately  measuring  the  surface  ship's 
heading,  pitch,  and  roll  also  become  vital,  especially  when  the  range  to  the  ROV 
increases,  since  eiTors  in  these  measurements  result  in  an  enoneous  measurement  of 
bearing  to  the  ROV  {Sonwrs,  1991). 

2.5.3  Ultrashort-Baseline  Systems 

The  ultrashort-baseline  system  uses  a  hydrophone  an-ay  mounted  either  on  the 
ROV  or  the  surface  ship,  with  only  a  single  pinger  on  the  other  platfonn.  Normally,  the 
hydrophone  an-ay  is  on  the  surface  ship.  Phase  comparisons  rather  than  travel-time 
measurements  are  used  to  detennine  the  position  of  the  vehicle.  The  acoustic  pinger  sends 
out  a  pulse,  and  the  processor  connected  to  tlie  hydrophone  array  measures  the  phase 
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difference  in  the  signals  received  at  each  hydrophone.  This  phase  difference  is  converted 
into  a  bearing  to  the  pinger.  For  tethered  vehicles,  the  time  of  travel  can  also  be  computed 
as  in  the  short-baseline  case  to  obtain  range.  Therefore,  ROV  position  can  be  determined. 
To  measure  phase  difference  accurately,  the  receiving  hydrophones  are  mounted  within 
one  wavelength  of  each  other  (typically,  only  cenumeters  apart),  making  the  array  small 
enough  for  use  on  ROVs. 

This  system  has  two  major  disadvantages.  First,  the  installation  must  be  extremely 
precise,  which  can  be  difficult.  Second,  multipath  arrivals  of  the  sound  waves  via  bottom- 
bounce  or  surface-reflection  can  cause  false  phase  measurements  resulting  in  large 
inaccuracies.  In  his  1992  thesis,  Brian  Tracey  explores  techniques  to  minimize  the  effects 
of  this  multipath  interference  (Tracery,  1992). 

2.5.4  Other  Positioning  Systems 

Besides  traditional  acoustic  positioning  systems,  other  possibilities  for 
determining  vehicle  position  exist.  For  example,  research  is  being  performed  at  DSL  and 
other  laboratories  in  making  teiTain-relative  mapping  feasible.  This  method  entails 
matching  bottom  features  seen  by  the  vehicle  with  known  features  from  an  existing 
database,  or  with  features  already  found  and  mapped  by  the  vehicle.  Depending  on  the 
features  present  in  the  vicinity,  the  quality  of  the  databa.se,  and  such  system  characteristics 
as  sonar  frequency,  position  fixes  with  accuracies  on  the  order  of  meters  are  foreseeable. 

2.5.5  Position  Information  Required  for  Use  in  the  Kalman  Filter 

For  the  pui-poses  of  constructing  the  filter,  any  of  these  system  configurations  can 
be  used,  but  allowances  must  be  made  to  account  for  the  differences  between  them.  A 
longer  range  system,  using  lower  frequencies,  is  typically  less  accurate.  Additionally, 
since  the  frequency  of  fixes  is  limited  by  the  speed  of  sound  in  the  water,  the  longer  range 
system  will  obtain  fixes  less  frequently.  The  filter  must  be  adjusted  accordingly. 
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2.6  Gyro 

The  gyro  used  at  DSL  is  manufactured  by  Humphrey,  Inc.  It  is  a  standard 
uncompensated  model  that  provides  continuous  heading  infonnation.  It  maintains 
excellent  short-term  precision  with  little  "jitter,"  or  high-frequency  fluctuation.  However, 
because  it  is  uncompensated,  it  has  a  high  drift  rate  that  tends  to  fluctuate  from  three  to  six 
degrees  per  hour.  This  drift  introduces  a  significant  and  variable  bias  that  can  have  a 
debilitating  effect  on  the  Kalman  filter,  which  assumes  only  zero-mean  Gaussian  errors. 
To  correct  for  this  bias,  it  is  included  in  the  filter  state  space  so  that  it  can  be  condnuously 
adjusted  as  necessary  based  on  other  heading  measurements. 

2.7  Flux-Gate  Magnetic  Compass 

The  magnetic  compass  used  at  the  DSL  is  the  C-lOO,  manufactured  by  KVH 
Industries,  Inc.  It  consists  of  a  toroidal  fiux-gaie  sensing  element  with  an  associated 
electronics  board.  The  sensor  element  is  a  saturable  ring  core,  which  floats  in  an  inert  fluid 
so  the  sensing  element  remains  horizontal.  Windings  suiTOund  the  lexan  housing  of  the 
sensor  element,  electrically  driving  the  coil  into  saturation.  Secondary  windings  then 
sense  pulses  generated  by  the  horizontal  component  of  the  earth's  magnetic  field  (KVH, 
1992). 

The  compass  provides  heading  information  at  10  Hz.  The  manufacturer's 
specifications  list  the  accuracy  as  0.5°,  with  a  0.1°  resolution.  However,  there  are  three 
major  obstacles  to  achieving  this  accuracy  when  the  compass  is  mounted  on  a  vehicle. 

First,  the  magnetic  compass  is  referenced  to  magnetic  north  rather  than  true  north. 
Therefore,  the  difference  between  true  and  magnetic  noith,  or  variation,  must  be 
compensated  for  prior  to  using  the  heading  infonnation  for  the  filter.  This  variation  is 
dependent  on  the  location  of  the  vehicle.  While  the  average  variation  in  an  area  can  easily 
be  obtained  from  the  pertinent  navigational  chart,  the  actual  compass  reading  can  be 
affected  by  local  concentrations  of  iron  beneath  the  bottom  of  the  sea.  Since  the  vehicle  is 
usually  operating  within  200  m  of  the  ocean  bottom,  where  local  magnetic  disturbances 
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can  be  present,  performance  of  the  magnetic  compass  must  be  monitored  to  ensure  that 
any  magnetic  disturbances  ai-e  recognized. 

The  second  en^or  is  deviation,  or  the  difference  between  a  perfect  magnetic 
compass  and  the  compass  in  use.  This  is  influenced  by  the  construction  of  the  compass  as 
well  as  the  presence  of  feiTous  metals  on  the  vehicle  in  the  vicinity  of  the  compass. 

The  third  source  of  en'or  is  more  problematic.  Whenever  the  vehicle's  thrusters  are 
used,  the  resulting  magnetic  field  from  the  energized  motors  affects  the  output  of  the 
compass,  resulting  in  output  instability  until  the  thruster-induced  magnetic  transients 
decay.  For  large-area  mapping  operations,  the  vehicle  operates  mosdy  on  long,  straight 
legs,  so  the  effects  of  thruster-induced  en^ors  are  minimized.  Operations  requiring  more 
frequent  maneuvering  degrade  compass  accuracy  accordingly. 

The  characteristics  of  the  magnetic  compass  are  the  opposite  of  the  gyro:  it  does 
not  drift  with  time  but  does  exhibit  significant  shori-tenn  fluctuations.  Becau.se  of  its  lack 
of  drift,  it  can  be  used  as  a  reference  to  reset  the  gyro  when  required.  However,  care  must 
be  taken  to  ensure  that  the  magnetic  compass  is  settled  out  when  performing  the  reset. 
Most  importantly,  the  vehicle  thrusters  should  not  be  in  operation  during  this  procedure  to 
avoid  the  subsequent  error  in  compass  heading  output.  As  for  the  gyro,  magnetic  compass 
bias  is  included  in  the  state  .space  to  provide  a  continuously  updated  estimate  of  its  value. 


2.8  Inclinometer 

At  DSL,  the  single-axis  Wat.son  inclinometer  with  angular  rate  sensor  is  u.sed  to 
provide  pitch  and  roll  information.  This  unit  combines  accuracy  and  reliability  with  small 
size  and  low  power  requirements,  making  it  ideal  for  vehicle  operations. 

The  inclinometer  uses  a  rate  sensor  and  an  integral  vertical  reference.  A  position 
output  is  obtained  by  integrating  the  angular  rate  output.  By  comparing  this  signal  with  the 
vertical  reference,  an  eiTor  signal  is  generated.  This  error  signal  is  filtered  and  sent  back  to 
the  rate  sensor  as  a  bias.  The  system  is  also  damped.  With  the  overall  sy.stem,  the  effects  of 
inertia,  damping,  and  short-term  accelerations  are  reduced. 

The  angular  rate  sensor  uses  two  piezoelectric  bender  elements,  which  are 
resonantly  driven  in  opposite  directions.  Rotation  causes  a  bending  force,  which  is 
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Fig.  2-7.  Accelerometer  sensor  exploded  view  (Cady,  1984). 

demodulated  to  produce  the  rotation  rate.  The  veitical  reference  uses  a  liquid-capacitive 
element.  With  these  components,  the  system  has  no  moving  parts,  which  increases 
reliability.  Overall,  the  system  weighs  less  than  eight  ounces,  making  it  easy  to  mount  on  a 
ROV  (Watson,  1990).  To  obtain  the  necessary  data,  two  of  these  units  are  used  on  the 
vehicle,  mounted  orthogonally.  One  provides  pitch,  the  other  roll. 


2.9  Systron  Donner  MotionPak  (Inertial  Motion  Unit) 

The  Systi'on  Donner  MotionPak  provides  the  final  sensor  measurements  required 
for  the  Kalman  filter.  It  consists  of  two  pans:  the  linear  accelerometers  and  the  angular 
velocimeters. 

2.9.1  Accelerometer  Operation 

The  linear  accelerometer  used  for  vehicle  navigation  in  this  application  is  the 
Sundstrand  Data  Control  Q-Flex®  accelerometer  (see  Fig.  2-7).  Acceleration  produces  a 
torque  on  the  sensor's  proof  mass.  A  detector  measures  the  displacement  of  the  mass  and 
produces  a  proportional  output  voltage.  The  resultant  signal  is  amplified  and  fed  to  a 
torquer  coil  fixed  to  the  proof  mass.  This  current  in  the  coil  provides  a  restoring  torque  to 
balance  the  applied  acceleration.  The  current  also  goes  through  a  load  resistor,  generating 
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Fig.  2-8.  Accelerometer  system  functional  diagram  {Cady,  1984). 

the  output  voltage  for  the  detector  {Cody,  1984).  See  Fig.  2-8  for  an  overall  system 
schematic. 

2.9.2  Angular  Velocity  Measurements 

Angular  measurements  are  provided  within  the  IMU  using  the  Systron  Donner 
GyroChip''''''^  This  device  uses  a  vibrating  quartz  tuning  fork  to  sense  angular  rate  by 
acting  as  a  CorioHs  sensor.  The  vibrating  fork  drives  a  similar  pickup  fork  that  produces 
the  output  signal.  These  two  forks,  along  with  their  support  flexures  and  frame,  are  made 
from  a  wafer  of  single-crystal  piezoelecUMC  quartz.  The  drive  tines  are  driven  by  an 
oscillator  that  causes  the  tines  to  move  toward  and  away  from  each  other  at  high 
frequency.  Each  tine  has  a  coriolis  force  acting  on  it  given  by 

F  =  2mcox  V/-, 

where 

m  is  the  tine  mass, 
CO  is  the  input  rate, 
Vy  is  the  instantaneous  linear  radial  velocity. 

The  forces  generated  are  perpendicular  to  the  plane  of  the  fork  assembly  at  each  of 
the  tines  and  in  opposite  directions,  yielding  a  torque  proportional  to  the  input  angular 
velocity.  The  pickup  tines  respond  to  the  oscillating  torque  by  moving  in  and  out  of  the 
plane,  causing  output  signals  to  be  produced  by  the  pickup  amplifier  (Systron  Donner, 
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1993).  A  separate  unit  is  provided  for  each  axis,  resulting  in  independent  measurements 
for  pitch  rate,  roll  rate,  and  yaw  rate. 
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Chapter  3 

Development  Of  The  Kalman  Filter 
Model 


3.1  Characteristics  of  the  Linear  Kalman  Filter 

The  purpose  of  the  Kahnan  filter  is  to  produce  an  unbiased,  minimum  variance, 
consistent  estimate  of  a  quantity.r  based  on  a  set  of  measurements,  z,  where  the 
relationship  between  these  variables  can  be  represented  by 

Z  =  Hx  +  V . 

Therefore, z  is  a  linear  combination  of  the  elements  of  the  vector  .v,  plus  random  noise 
represented  asv  {Gelb,  1974).  The  desired  properties  of  such  an  estimate,  as  delineated 
above,  are: 

Unbiased:  the  expected  value  of  the  estimate  is  the  same  as  that  of  the  actual  quantity 
being  esdmated. 

Minimum  Variance:  the  error  distribution  of  the  estimate  is  less  than  or  equal  to  that  of 
any  other  unbiased  estimator. 

Consistent:  as  the  number  of  measurements  increases,  the  estimate  converges  to  the 
true  value  of  the  quantity  being  estimated. 

If  the  noise  is  Gaussian  and  the  relationship  is  linear,  the  Kalman  filter  has  been  proved  to 
meet  these  criteria  (Gelb,  1974). 

An  additional  characteristic  of  the  Kalman  Filter  is  that  it  is  recursive,  which 
means  that  new  esfimates  can  be  computed  without  storing  past  measurements.  Instead,  all 
previous  information  is  summarized  in  the  current  estimate  and  associated  error 
covariance  matrix.  This  recursiveness  allows  real-time  processing  of  data  without 
excessive  memory  storage  requirements  or  computational  burden. 
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3.1.1  System  Model 

For  a  given  real  system  to  be  represented  using  a  Kalman  filter,  it  must  be  put  into 
a  standard  form.  The  two  governing  equations  of  the  system  model  are: 

X  =  A.X  +  Gw  +  Lu 

and 

z  =  C.x  +  Dij  +  V, 

where?/  is  a  deterministic  control  input, m'  is  a  random  forcing  function  with  error 
covariance  Q  ,  and v  is  the  measurement  noise  with  enor  covariance  matrix  R  .  A,G,  L,  C, 
and  D  are  matrices  describing  the  relationship  between  the  different  vectors. 

The  first  equation  states  that  the  rate  of  change  of  the  system  parameters  can  be 
predicted  using  a  linear  combination  of  the  present  pai'ameters  and  a  linear  combination  of 
control  inputs.  The  second  states  that  the  measurement  vector,  z ,  is  a  linear  combination 
of  the  system  parameters  plus  a  linear  combination  of  the  present  control  inputs.  Both 
equations  include  the  effects  of  noise  by  virtue  of  then'  andv  terms. 

3.1.2  System  Model  Dynamics 

To  produce  an  appropriate  Kalman  filter  model,  the  stale  variables  that  make  up 
the  vectors.!'  andz  must  be  defined.  The  vector.v  includes  all  parameters  necessary  to 
model  vehicle  behavior.  The  vectorz  includes  all  measurements  that  can  be  obtained  from 
the  sensors  as  described  in  Chapter  2.  Both  must  be  determined  using  knowledge  of  the 
system  dynamics  and  the  available  sensors.  In  this  case,  there  are  two  different  types  of 
model  propagation,  one  for  vehicle  motion  and  one  for  attitude. 

The  primary  control  forces  are  those  induced  on  the  vehicle  by  the  tow  cable. 
However,  these  are  virtually  impossible  to  predict  due  to  the  realities  of  the  towing 
operation.  First,  although  the  surface  ship  is  proceeding  predominantly  on  a  known 
course,  wave  action  produces  continuous  variations  in  pitch,  roll,  and  heading.  These 
variations  are  all  propagated  down  the  tow  line  to  some  degree,  despite  various  techniques 
used  to  isolate  vehicle  motion  from  ship  motion.  Even  if  these  forces  on  the  tow  cable  at 
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Fig.  3-1.  System  model  for  X-posilion  propagation. 

the  surface  could  be  measured  precisely,  the  cable  forces  at  the  vehicle,  several  thousand 
meters  away,  are  difficult  to  model. 

The  vehicle  model  is  also  ill-defined.  Changes  in  equipment  configuration  on  the 
vehicle,  which  affect  drag  and  added  mass,  can  have  significant  effects  on  the  vehicle 
dynamics.  Also,  when  cable  forces  induce  a  change  in  vehicle  attitude,  the  force  of  the 
water  acting  on  the  moving  vehicle  coupled  with  the  designed  self-righting  characteristics 
of  the  vehicle  combine  to  restore  the  vehicle  to  its  mean  velocity  and  attitude.  Thus,  the 
vehicle  frequently  follows  a  damped  sinusoidal  path  in  both  vertical  and  horizontal 
directions.  Therefore,  the  predominant  characteristics  of  vehicle  motion  are: 

1.  It  is  extremely  unpredictable  due  to  the  inability  to  measure  cable  forces  and  to 
properly  model  vehicle  dynamics. 

2.  The  vehicle  tends  to  return  to  its  mean  attitude  after  a  cable  disturbance  has  sub- 
sided. 

As  a  result,  the  vehicle  accelerations  are  be  considered  to  be  influenced  by  a  white- 
noise  disturbance  and  control  inputs  are  not  modelled.  To  help  compensate  for  any  errors 
in  these  assumptions,  linear  accelerations  are  measured  by  the  strapdown  inertial  motion 
unit  (IMU)  to  provide  cun-ent,  though  noisy,  actual  accelerations  to  the  filter  algorithm. 

For  thruster-controlled  vehicles,  control  input  is  often  better  defined.  In  these 
circumstances,  operator-controlled  inputs  can  be  added  as  a  deterministic  vector  u ,  with 
an  associated  matrix  L  to  mathematically  relate  the  inputs  to  their  effect  on  the  state 
vector. 

To  estimate  vehicle  position,  the  system  model  is  as  shown  in  Figure  3-1.  X- 
position  (latitude)  is  shown;  Y-position  (longitude)  and  Z-position  (depth)  follow  the  same 
principles. 
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The  system  model  for  the  angular  positions  is  similar  but  requires  only  one 
integrator.  Again,  the  system  is  modelled  as  disturbed  by  white  noise.  The  angular 
velocity  output  of  the  IMU  is  used  as  the  measurement  for  angular  velocities  (roll,  pitch, 
and  heading),  shown  in  Figure  3-2. 


3.2  Sensor  Outputs  and  Their  Relationship  to  the 
State  Space 

Before  proceeding  to  the  specifics  of  the  state  space  model  for  this  application, 
some  details  of  the  sensor  data  output  must  be  described.  To  produce  a  useful  Kalman 
filter  for  this  application,  it  is  essential  to  identify  exactly  what  information  is  available. 
Additionally,  an  expected  frequency  for  receipt  of  data  by  the  filter  is  necessary. 

During  vehicle  operations  conducted  by  DSL,  four  different  data  packages  are 
collected  as  the  appropriate  data  are  available.  These  four  data  streams  must  be  translated 
into  a  format  usable  by  the  Kalman  filter,  and  any  potential  gaps  in  receiving  data  must  be 
anticipated  and  accounted  for  in  the  filter  algorithm. 

3.2.1  IMU  Package 

The  IMU  package  consists  of  the  linear  accelerometers  and  angular  velocimeters. 
As  is  the  case  for  the  other  three  data  packages,  the  raw  data  stream  is  translated  into  a 
usable  form  for  tjhe  filter  using  a  relatively  simple  program  written  in  C  language.  The 
IMU  package  provides  data  at  a  rate  of  approximately  10  Hz.  The  IMU's  outputs  are  all  in 
sensor  coordinates,  which  must  be  converted  to  body  coordinates  for  use  in  the  filter. 
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3.2.2  Attitude  Package 

The  attitude  package  combines  the  outputs  of  the  depth  detector,  inclinometers, 
gyro,  and  compass  to  provide  depth,  roll,  pitch,  and  heading.  While  individual  sensors 
providing  data  operate  at  various  frequencies,  the  overall  package  operates  at 
approximately  5  Hz. 

3.2.3  Doppler  Velocimeter  Package 

Included  in  each  data  stream  from  the  doppler  velocimeter  is  considerable  detail 
about  the  operator-selected  parameters  being  used  by  the  velocimeter.  However,  only  the 
body-referenced  velocities  in  each  onhogonal  direction  and  the  velocity  error 
measurement  are  used  by  the  Kalman  filter.  Again,  a  C  program  is  used  to  translate  the 
Hex-ASCII  output  into  usable  data,  and  the  sensor-referenced  outputs  must  be  converted 
to  body  coordinates. 

As  discus.sed  in  Chapter  2,  the  output  frequency  of  the  doppler  velocimeter 
depends  primarily  on  the  number  of  pings  per  ensemble  and  the  altitude  of  the  vehicle 
above  the  bottom.  For  the  sidescan-sonar  mapping  configuration  employed  during  the 
DSL- 120  deployment  used  for  this  thesis,  the  doppler  velocimeter  usually  provided  data 
approximately  every  2-3  s.  However,  this  time  period  was  occasionally  shortened  or 
lengthened  due  to  changing  altitude  and  varying  bottom  conditions. 

3.2.4  Positioning  System  Package 

The  positioning  system  has  the  most  irregular  of  the  four  data  streams  received  for 
vehicle  navigation.  Frequency  of  data  reception  depends  on  the  type  of  positioning  system 
in  use.  For  systems  using  an  acoustic  transponder  array,  such  as  the  long-baseline  or  shon- 
baseline  systems,  this  depends  primarily  on  the  distance  from  the  vehicle  to  the  acoustic 
transponder  aiTay.  However,  whenever  data  acquisition  is  secured  (usually  when  a  given 
track  line  is  completed  until  the  towing  ship  has  maneuvered  to  get  into  position  for  the 
next  track  line),  the  positioning  system  is  frequently  secured  as  well,  since  precise 
knowledge  of  vehicle  location  is  unnecessary  except  when  obtaining  sonar  data.  Of  more 
concern  is  a  loss  of  position  information  during  sonar  operations.  There  are  several 
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different  possibilities  tiiat  can  cause  such  a  problem.  A  large  bottom  feature,  such  as  a 
ridge,  can  physically  block  the  path  from  the  acoustic  array  to  the  vehicle.  Another 
significant  factor  is  the  sound-velocity  profile  (SVP),  which  dictates  the  path  sound  waves 
will  travel  in  the  water.  With  an  unfavorable  SVP,  acoustic  position  information  can  be 
lost  only  a  short  distance  away  from  the  transponder  array. 

For  systems  not  tied  to  an  array,  such  as  a  terrain-relative  positioning  system,  the 
frequency  of  obtaining  information  will  be  highly  unpredictable  since  it  depends  on  the 
available  terrain  and  the  quality  of  the  database.  It  is  likely  that  there  will  be  frequent  long 
gaps  between  fixes  and  that  these  gaps  will  exist  whether  or  not  sonar  operations  are  in 
progress. 

This  irregularity  of  position  information  is  an  imponani  rea.son  for  developing  a 
Kalman  filter  using  the  doppler  velocimeter  infonnation.  By  filling  in  the  gaps  in  posiuon 
fixes  with  an  accurate  estimate  of  vehicle  position,  the  .sonar  data  quality  can  be  gready 
enhanced. 

Like  the  attitude  data  stream,  the  position  data  stream  is  typically  easy  to  decipher. 
The  components  used  for  this  Kalman  filter  are  the  X  and  Y  positions  (in  meters  from  a 
reference  point)  and  the  time  the  datuin  was  obtained. 


3.3  Defining  the  State  Space 

Now  that  the  system  dynamics  and  available  measurements  are  clearly  idenfified, 
the  state  space  can  be  defined.  Once  the.v  vector  is  labeled,  the  Kalman  filter  model  can  be 
mathemancally  developed.  Using  the  coordinate  system  axes  specified  in  Secfion  2.2,  the 
components  of  the  state  vector.v  are  defined  as: 

XI:  X-acceleration  (All  accelerations  are  in  body,  or  vehicle-referenced,  coordinates) 

X2:  Y-accelej-ation 

X3:  Z-acceleration 

X4:  X-velocity  (All  velocities  are  in  body,  or  vehicle-referenced,  coordinates) 

X5:  Y-velocity 

X6:  Z-velocity 
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X7:  X-position  (All  positions  are  in  earth-referenced  coordinates) 

X8:  Y-position 

X9:  Z-position 

XIO:  Roll  rate  (Defined  as  rotation  about  the  vehicle's  Y-axis;  positive  roll  is  defined 
as  port  side  up) 

XU:  Pitch  rate  (Defined  as  rotation  about  the  vehicle's  X-axis;  positive  pitch  is 
defined  as  bow  up) 

X12:  Yaw  rate  (Defined  as  rotation  about  the  vehicle's  Z-axis;  heading  is  measured 
with  normal  compass  orientation.  To  maintain  the  right-handedness  of  the  system,  this 
is  converted  within  the  Kalman  filter  algorithm  to  measure  positive  heading  rate, 
usually  called  yaw  rate,  as  counterclockwise,  rather  than  the  nonnal  compass  direction 
of  clockwise) 

X13:Roll 

XI 4:  Pitch 

X15:  Heading 

X16:  Gyro  bias  (Difference  between  gyro  heading  and  tiiie  heading) 

X17:  Magnetic  compass  bias  (Difference  between  compass  heading  and  true  heading. 
It  includes  both  vaiiation  and  deviation) 

Also,  there  is  the  measurement  vector,  r.  When  all  four  data  streams  are  available  to  the 
filter,  the  components  ofz  coiTespond  exactly  to  the  components  of  the  state  vector  x, 
with  the  addition  of  two  heading  measurements  from  the  gyro  and  compass.  As  discussed 
later,  however,  this  is  seldom  the  case  due  to  the  variations  in  data  stream  frequency. 


3.4  Nonlinearity  and  the  Extended  Kalman  Filter 

Unfortunately,  the  differences  in  coordinate  systems  between  the  various  sensors 
means  that  the  model  cannot  be  treated  as  linear.  With  vehicle  accelerations  and  velocities 
provided  in  body  coordinates  and  positions  provided  in  earth-referenced  coordinates,  a 
coordinate  transformation  must  be  made  between  the  two  references  to  allow  the  data  to 
be  used.  This  coordinate  transformation  is  accomplished  using  standard  trigonometric 
equations  that  use  vehicle  roll,  pitch,  and  heading  to  relate  body  coordinates  to  eaiih 
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coordinates.  This  transfonnation  is  peifonned  within  the  extended  Kahnan  filter  algorithm 
by  altering  the  /\  matrix  at  each  filter  iteration  as  explained  in  Section  3.7.2.2 

3.4.1  Choosing  the  Extended  Kalman  Filter 

As  stated  in  Section  3.1,  the  linear  Kalman  filter  has  been  proved  to  provide  an 
unbiased,  minimum-variance,  consistent  estimate  of  the  state  vectorx  as  long  as  the  noise 
is  Gaussian.  Noise  considerations  are  addressed  in  Section  3.5. 

To  compen.sate  for  the  nonlinearity  of  the  system,  several  techniques  are  available. 
Unfortunately,  none  of  these  can  be  proved  to  meet  the  same  standards  as  the  linear 
Kalman  filter.  In  order  of  complexity,  three  possible  solutions  are  the  extended  Kalman 
filter,  the  iterated  extended  Kalman  filter,  and  the  second-order  filter  (Gelb,  1974). 
Because  it  is  the  simplest  and  incurs  the  least  computational  burden,  the  extended  Kalman 
filter  is  used  to  provide  the  state  estimates  for  this  application.  As  Gelb  states. 

There  is  no  guarantee  that  the  actual  estimate  obtained  will  be  close  to  the  truly 
optimal  estimate.  Fortunately,  the  extended  Kalman  filter  has  been  found  to  yield  accurate 
estimates  in  a  number  of  important  practical  applications  {Gelb,  1974). 

3.4.2  Filter  Propagation 

The  basic  method  of  producing  a  slate  estimate  with  the  extended  Kalman  filter  is 
the  same  as  for  the  linear  version.  First,  some  definitions  must  be  provided: 

.r  {t\t)   is  the  estimate  of  the  .state  vector  using  all  data  up  to  and  including  the  present 
time. 

x{t  +  \\t)   is  the  predicted  e.stimate  of  the  state  vector  at  the  time  of  the  next  filter 
update  step  using  all  data  up  to  and  including  the  present  time. 

x{t\t-l)   is  the  predicted  estimate  of  the  state  vector  at  the  present  time  using  data 
up  to  and  including  tiie  time  of  the  last  filter  step.  It  is  the  same  as  the.v  {t  +  \\t) 
calculated  after  the  previous  filter  update  step. 

P  {t\t)   is  the  eiTor  covariance  matrix  of  the  state  vector  using  all  data  up  to  and 
including  the  present  time. 

P  {t  +  \\t)   is  the  predicted  covariance  matrix  of  the  state  vector  at  the  time  of  the  next 
filter  update  .step. 

P  (t\t-\)   is  the  predicted  estimate  of  the  error  covariance  matrix  at  the  present  time 
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using  data  up  to  and  including  the  time  of  the  last  filter  step.  It  is  the  same  as 
theP  (r  +  \\t)  calculated  after  the  previous  filter  update  step. 

For  the  linear  Kalman  filter,  the  error  covariance  matrices  are  deterministic  and 
exact  based  on  the  assumptions  used  for  input  and  measurement  noise  in  the  filter  model. 
Therefore,  they  provide  the  current  uncertainty  of  each  variable  in  the  state  vector, 
assuming  the  model  is  correct.  For  the  extended  Kalman  filter,  the  error  covariance  is  only 
an  approximation  since  the  actual  eiTor  covariance  matrix  cannot  be  calculated  due  to  the 
nonlinearity  of  the  system,  a  condition  that  can  theoretically  cause  filter  divergence. 
However,  the  extended  Kalman  filter  has  been  found  to  work  in  numerous  practical 
applications,  with  the  only  test  being  whether  the  filter  works  in  actual  use  (Gelb,  1974). 
As  demonstrated  in  Chapter  4,  the  good  results  using  simulated  and  actual  data  support  the 
assumption  that  the  extended  Kalman  filter  is  an  appropriate  choice  for  this  applicadon. 

Forfilter  propagation,  current  estimates  of.v(r|/-  1)   D.ndP  {t\t-  1)  are  available 
from  the  previous  filter  iteration.  As  explained  in  Section  3.1.1,  the  governing  equations 
for  the  system  model  are: 

-V  =  A-X  +  G\v 

and 

z  =  Cx  +  Dij. 

ThQDu  term  is  called  the  feed-forward  tenn  and  represents  the  immediate  effect  of 
control  inputs  on  output.  As  discussed  earlier,  control  inputs  are  not  modeled. 

3.4.2.1  Update  Step 

The  first  step  in  the  filter  propagation  is  to  detennine  the  innovation  term  /,  which 
is  defined  by  the  equation: 

/  =  .--  (Cx.v(r|r-1)). 

This  is  the  difference  between  the  expected  measurement  vector  obtained  from  the  state 
esdmate.r  (f  +  \\t)  calculated  during  the  previous  filter  update  step  and  the  actual 
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measurement  vector  derived  from  the  vehicle's  sensors.  Next,  the  covariance  matrix  of 
innovation,  v ,  is  calculated: 

V  =  CxP{t\t-  1)  xC'  +  /?, 

where/?  is  the  error  covariance  matrix  of  the  measurement  vector,  z .  The  Kalman  gainA: 
is  then  calculated: 

K  =  P(t\t-\)  xCxinviv)  . 

The  next  step  is  to  calculate  the  new  state  vector  estimate  using  the  value  of.v  (r  +  l\t) 
determined  during  the  previous  filter  update  step: 

x{t\t)    =  .\it\t-\)  +KxI. 

Finally,  the  new  eiTor  covariance  is  computed: 

P{t\t)    =   (Iclent-KxC)  xP{t\t-\)  X  (fcfent-KxC)'  +  KxRxK'  , 

where  Ident  in  this  case  is  the  identity  matrix  with  the  same  dimensions  as  KxC . 

3.4,2.2  Prediction  Step 

For  the  standard  Kalman  filter,  the  predicted  values  of  the  state  vector  and  the  error 
covariance  matrix  for  the  next  time  step  using  data  up  to  the  present  time  are  now 
calculated.  Since  the  Kalman  filter  is  implemented  as  a  discrete  instead  of  continuous 
filter,  a  conversion  of  the  propagation  equations  from  their  continuous  fomis  given  in 
Section  3.1.1  to  their  discrete  counteipans  is  perfonned.  This  transfonnation  uses  the 
MATLAB  function  "c2d",  which  takes  the  A  and  G  matrices  and  the  filter  time  step  as 
inputs  and  provides  the  discrete  matrices(j)  (translation  of  the  A  matrix)  andf  (translation 
of  the  G  matrix)  as  outputs.  Once  the  conversion  to  discrete  matrices  is  made,  the 
predictions  are  computed: 

x{t+\\t)   =  ([)  x.v(/|r)  +  Fx  u'. 
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P{t+[\t)   =  (^xP{t\r)  x(^'  +  rxQxr' , 
where Q  is  the  error  covariance  matrix  for  the  white  noise,  vv . 

3.4.3  Implications  of  the  Extended  Kalman  Filter 

To  obtain  a  new  state  estimate,  the  extended  Kalman  filter  uses  a  first-order  Taylor 
series  expansion  about  the  current  state  estimate  to  linearize  the  problem  at  each  update 
step.  In  the  steady-state  version  of  a  linear  Kalman  filter,  the  Kalman  gain  A!"  is  a  constant. 
For  the  extended  Kalman  filter,  it  must  be  recomputed  at  each  update  step  {Gelb,  1974). 

3.5  Modeling  System  Noise 

Prior  to  using  the  Kalman  filter,  predictions  must  be  made  concerning  the 
estimated  errors  in  the  sensors  used.  All  data  used  by  the  filter  are  weighted  based  on  these 
predicted  en-ors.  A  measurement  considered  very  accurate  has  a  strong  effect  on  the  state 
estimate.  For  example,  if  position  information  is  considered  very  accurate  relafive  to 
heading  and  velocity,  the  estimates  of  the  latter  two  are  altered  from  their  measured  values 
as  necessary  to  ensure  that  the  estimate  of  position  is  close  to  the  measured  value.  Poor 
position  information  places  more  reliance  on  heading  and  velocity  measurements.  To 
ensure  optimum  filter  performance,  it  is  essential  that  the  errors  be  modelled  as  accurately 
as  possible. 

Another  important  assumption  in  the  Kalman  filler  is  that  any  eiTors  in  the  control 
input  vector  and  the  measurement  vector  are  unbiased  and  follow  a  Gaussian  distribution. 
For  nearly  all  cases,  the  assumption  of  Gaussian  errors  is  valid  considering  the 
characterisfics  of  the  sensors  employed.  The  prominent  exception  is  the  positioning 
system.  While  normally  the  errors  can  be  treated  as  Gaussian,  occasionally  the  system 
produces  anomalously  large  en^ors  in  position.  If  these  position  fixes  are  treated  by  the 
filter  as  valid,  improper  changes  in  the  elements  of  the  slate  vector  are  made,  resulting  in 
rapid  filter  degradation.  Identifying  and  ignoring  these  bad  data  is  an  integral  part  of  the 
modified  filter  algorithm. 
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3.5.1  Eliminating  Non-Gaussian  Position  System  Errors 

The  grossly  inaccurate  position  measurements  occasionally  produced  by  acoustic 
positioning  systems  are  frequently  refeiTed  to  as  "flyers".  Removing  them  from  the 
database  is  vital,  but  care  must  be  taken.  If  the  allowable  tolerance  between  estimated 
position  and  measured  position  is  too  tight,  valid  position  fixes  can  be  rejected  by  the 
filter.  Once  this  divergence  between  estimated  and  actual  position  occurs,  the  likelihood  of 
ever  accepting  another  position  fix  is  greatly  reduced.  This  problem  is  most  acute  when  a 
long  intei*val  exists  between  fixes.  If  vehicle  heading  or  velocity  is  estimated  inaccurately 
during  this  interval,  the  difference  between  estimated  and  actual  vehicle  posifion  grows.  If 
the  discrepancy  becomes  large  enough,  when  position  information  is  once  again  obtained 
it  is  considered  to  be  a  llyer  and  rejected.  With  no  further  position  information,  the  state 
estimate  of  position  becomes  increasingly  inaccurate. 

To  provide  a  window  that  is  likely  to  reject  fiyers  while  accepting  all  valid  fixes, 
the  standard  allowable  enxir  is  established  at  ten  times  the  standard  deviation  of  the 
position  system.  To  provide  for  time-dependent  system  errors,  an  addition  is  made  to  the 
allowable  error  based  on  the  time  since  the  last  posifion  system  fix,  given  by 

Addiuonal  allowable  error    =  O.lm  x  Ar, 

where 

At  =  Time  since  last  posifion  fix  in  seconds. 

Fixes  that  yield  a  difference  between  the  cuirent  estimated  position  and  the  position 
system  fix  of  greater  than  this  total  allowable  error  are  rejected.  The  method  for 
determining  the  standard  deviation  is  discussed  in  Section  3.5.3.2. 

The  decision  to  use  a  cutoff  point  of  ten  times  the  standard  deviafion  is  noi  based 
on  any  formula.  It  is  large  enough  to  admit  reasonable  data,  but  still  should  reject  any 
flyers.  To  help  prevent  filter  divergence  between  esUmated  posifion  and  valid  fix 
informafion,  a  count  is  maintained  within  the  program  of  the  number  of  rejected  fixes.  By 
monitoring  this  count  during  vehicle  operations,  (ilter  divergence  can  be  recognized  by 
confinuous  fix  rejection.  The  window  of  acceptance  can  then  be  adjusted  as  necessary  to 


45 


allow  the  filter  to  accept  new  fixes  and  thus  change  the  state  esumates  as  appropriate  to 
reflect  the  valid  position  information. 

3.5.2  Random  Forcing  Function  Noise 

The  random  forcing  function  noise  is  expressed  by  the  error  covariance  matrix  Q . 
It  is  a  6x6  matrix  since  there  are  six  terms  that  drive  the  system,  three  linear  accelerations 
and  three  angular  velocities.  It  is  all  zeros  except  on  the  main  diagonal  since  each  of  these 
parameters  is  considered  to  be  independent  of  the  others,  making  the  cross  correlation 
terms  zero. 

The  magnitude  of  the  en-or  covariance  terms  must  be  estimated  from  knowledge  of 
the  vehicle  dynamics  accounting  for  tow  ship  mofion,  tow  line  motion,  and  vehicle 
dynamics.  For  this  filter,  the  standard  deviation  of  the  linear  accelerations  is  set  at  0.2  m/ 
s  ,  and  that  of  the  angular  velocimeters  at  2  "As.  These  constants  are  incorporated  into  the 
KFSetup  program. 

3.5.3  Measurement  Uncertainty 

The  measurement  uncertainty  is  incoiporated  into  the  17x17  error  covariance 
matrix  R  .  The  portions  of  this  matrix  applicable  to  each  sensor  are  addressed  separately 
below. 

3.5.3.1  Linear  Accelerometer  Uncertainty 

The  uncertainties  in  the  linear  accelerometer  measurements  have  two  sources.  One 
is  the  noise  and  bias  inherent  in  any  instrument,  which  can  be  determined  by  test  or  by 
using  the  manufacturer's  specifications.  The  other  is  caused  by  the  displacement  of  the 
sensor  from  the  vehicle's  reference  point  for  the  body-referenced  coordinate  system. 

As  shown  in  Figure  3-3,  the  vehicle's  center  of  rotation  and  therefore  its  most 
convenient  body-coordinate  reference  point  is  the  point  of  attachment  to  the  tow  line. 
Assuming  the  vehicle  is  not  experiencing  an  actual  linear  acceleration  to  the  left  (-X 
direction),  the  X-acceleromeler  would  still  sense  acceleration  if  angular  velocity  in  the 
yaw  direction  (change  in  heading)  were  non-zero.  This  phenomenon  is  governed'by  the 
following  coupled  equations  {Catford,  197X): 
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Angular 

velocity  ^  Linear  acceleration  sensed  due  to  angular  velocity 

■  T  ^.^X^enter  of  rotation 

I 1  J  (tow  point) 


I  \,^-Cei 


IMU  Sensor 
Fig.  3-3.  Linear  accelerometer  output  caused  by  angular  velocity. 

a^  =  u  + qw -vr  +  Uj^i-q^-r'^)  +  ay{-r  +  pq)  +  a^{q+  pr)  +^'sine 
a-y  =  V  - pw  +  iir  +  a T(ir  +  qp)  +  (iy{- p^  -  r^)  +a~{-p  +  qr)  -^'sin(t)cos0 

a,  =  w  +  pv -qii  +  a  ^{- q  +  rp)  +  (iy{p  +  rq)  +  a  ^  {- p^  -  q'^)  -^t,'cos0cos({) 

where  a^ ,  ay ,  and  a-  are  the  outputs  of  the  accelerometers;  (ij^-,  (i^„  and  a-,  are  actual 
linear  accelerations;  //,  \\  and  vv  are  velocities;  (()  =  roll;  6  =  pitch;  p,  q,  r  are  the  angular 
velocities;  g  is  the  gravity  vector. 

To  simplify  the  filter  algorithm,  this  addition  to  linear  acceleration  measurements 
due  to  angular  velocities  is  ignored.  The  justification  is  twofold:  first,  for  a  towed  vehicle 
the  magnitude  of  the  linear  acceleration  caused  by  the  angular  velocities  is  small 
compared  to  actual  linear  accelerations;  second,  over  a  relatively  small  time  interval,  the 
angular  velocities  have  a  mean  of  zero  due  to  the  damped  sinusoidal  mofion  of  the  vehicle. 
Therefore,  any  errors  induced  by  this  simplification  are  cancelled  out  when  the  angular 
velocity  reverses  direction.  As  a  result,  the  standard  deviation  for  the  linear  acceleration 
measurements  are  set  at  0. 1  m/s~. 

3.5.3.2  Doppler  Velocimeter  Uncertainty 

As  part  of  its  data  stream,  the  doppler  velocimeter  provides  an  error  velocity.  This 
error  velocity  is  calculated  by  the  velocimeter  using  a  proprietary  RDI  Instalments 
program.  Factored  into  this  calculation  are  the  depth  cell  size,  the  altitude  of  the  vehicle 
above  the  bottom,  the  vehicle  velocity,  and  the  number  of  pings  per  ensemble  {RDI,  1993). 
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This  en-or  velocity  is  used  by  the  Kalman  filler  program  as  the  standard  deviation  for 
velocity  measurements.  Therefore,  these  elements  of  the/?  matrix  vary  with  each  filter 
update  step. 

3.5.3.3  Position  System  Uncertainty 

The  standard  deviation  for  acoustic  positioning  systems  is  determined  based  on 
operator  knowledge  of  the  system  and  takes  into  account  such  parameters  as  system 
frequency  and  distance  from  the  transponder  airay.  It  is  established  prior  to  filter  operation 
as  part  of  the  KFSetup  program  but  can  be  changed  during  operations  if  desired.  For  other 
systems,  such  as  terrain-relative  position  fixes,  the  standard  deviation  can  be  made  part  of 
the  data  stream  based  on  the  estimated  accuracy  of  the  fix  and  can  therefore  be  vaiied  from 
fix  to  fix. 

Of  all  the  measurement  uncertainties,  this  requires  the  greatest  attention.  If  it  is 
assigned  to  be  too  large,  the  Kalman  filter  does  not  weight  position  fixes  enough,  resulting 
in  potentially  significant  eirors  in  the  estimated  position.  This  can  adversely  affect  the 
quality  of  any  sidescan-sonar  or  video  data  obtained  during  vehicle  operations. 
Conversely,  if  it  is  set  too  small  there  is  an  increased  danger  of  creating  sufficient 
divergence  between  the  estimated  and  actual  positions  to  cause  invalid  position  fix 
rejections. 

3.5.3.4  Depth  Sensor  Uncertainty 

When  operating  in  deep  water,  the  depth  sensor  has  one  important  drawback: 
because  of  the  limitations  of  its  computer  bit  capacity,  it  can  only  resolve  depth  to  the 
nearest  0.  Im.  Coupled  with  the  normal  fluctuations  of  the  instrument,  this  tends  to  make 
the  output  vary  frequently  within  a  ().4-m  band.  Therefore,  the  standard  deviation  for 
deep-water  operations  is  set  at  O.lm,  which  places  nearly  all  fiuctuations  within  two 
standard  deviations  of  the  actual  value.  This  standard  deviation  can  be  reduced 
appropriately  for  shallow-water  operations  when  the  resolution  is  higher. 
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3.5.3.5  Angular  Velocimeter  Uncertainty 

Estimation  of  the  angular  velocimeter  uncertainty  is  fairly  straightforward.  Prior 
testing  or  manufacturer's  speciiicalions  are  used  to  determine  the  expected  standard 
deviation.  For  the  instruments  in  use,  this  is  set  at  ().5"/s. 

3.5.3.6  Pitch  and  Roll  Uncertainty 

Like  the  depth  sensor,  the  pitch  and  roll  sensors  are  digital  and  therefore  have  a 
noticeable  minimum  resolution.  For  these  sensors  this  resolution  is  0.1",  and  normal 
fluctuations  in  a  0.2"  band  are  observed.  To  account  for  unmeasurable  biases  and  to 
prevent  the  filter  from  weighting  these  measurements  too  heavily,  a  standard  deviation  of 
0.5"  is  assigned. 

3.5.3.7  Heading  Uncertainty 

Heading  is  measured  directly  by  the  magnetic  compass  and  the  gyro.  For  each,  the 
applicable  fonnula  is 

True  Heading  =  Indicated  Heading  -  Sensor  Bias  . 

Since  the  bias  for  each  sensor  is  modeled  within  the  Kalman  filter  as  a  state  variable, 
additional  measurements  improve  the  estimate  of  bias  and  thereby  make  the  estimate  of 
true  heading  more  accurate. 

The  standard  deviation  of  the  magnetic  compass  is  constant  and  is  set  at  0.5"  to 
account  for  nornial  (luctuations.  If  frequent  thruster  operations  are  envisioned,  this 
standard  deviation  may  have  to  be  increased  due  to  the  effects  of  the  magnetic  fields 
induced  during  thruster  operations.  Due  to  its  greater  stability,  the  gyro  has  a  constant 
standard  deviation  set  at  0.1". 

To  provide  an  independent  measurement  of  true  heading,  which  is  necessary  to 
correct  sensor  biases,  two  different  techniques  are  possible.  For  a  highly  maneuverable 
vehicle  operating  in  a  small  area,  an  acoustic  transponder  can  be  mounted  both  at  the  front 
and  at  the  back  of  the  vehicle.  By  comparing  the  positions  of  each  transponder,  true 
heading  can  be  derived.  The  standard  deviation  for  this  technique  depends  on  the  accuracy 
of  the  positioning  system  used. 
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Fig.  3-4.  Calculation  of  true  heading  and  standard  deviation. 

For  a  towed  vehicle,  vehicle  motion  can  be  assumed  to  be  nearly  entirely  in  the 
direction  of  the  vehicle's  front.  Therefore,  a  measurement  of  true  heading  is  obtained  by 
calculating  a  least-squares  fit  to  the  last  four  position  fixes.  Although  the  standard 
deviation  of  this  heading  measurement  is  diflicult  to  calculate  precisely,  an  approximation 
is  derived  as  follows  (see  Fig.  3-4). 

After  obtaining  the  least-squares  solution,  a  separate  heading  measurement  is 
made  between  each  adjacent  pair  of  fixes  (1  and  2,  2  and  3,  3  and  4).  The  difference 
between  each  of  these  heading  measurements  and  the  overall  least-squares  measurement 
is  averaged.  This  average  difference  in  heading  measurements  is  considered  to  be  the 
standard  deviation  of  the  measurement.  Although  this  is  not  precise,  it  has  the  desired 
property  of  providing  less  standard  deviation  as  the  fix  measurements  become  less 
scattered  from  the  best-fit  track. 

3.6  Establishing  Reference  Points  and  Initial  Conditions 

3.6.1Reference  Points 

To  ensure  correct  matrix  manipulations  while  avoiding  round-off  errors,  MATLAB 
works  better  with  well-condiUoned  matrices,  which  means  that  the  elements  of  the 
matrices  are  as  close  together  as  possible  in  terms  of  order  of  magnitude.  To  achieve  this 
conditioning,  reference  points  for  position  and  time  must  be  established.  These  reference 
values  are  subtracted  from  the  actual  values  pv'wv  to  use  in  the  filter.  The  filter  uses  only 
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the  difference  between  the  reference  values  and  the  cuneni  values,  resulting  in  numbers 
smaller  by  several  orders  of  magnitude. 

For  X-  and  Y-positions,  the  Universal  Transverse  Mercator  (UTM)  system  is  used. 
This  system  divides  the  oceans  into  rectangular  grids  with  scales  in  meters.  A  suitable 
reference  point  is  chosen  in  the  area  of  vehicle  operations  and  included  in  the  KFSetup 
program. 

The  Z-position  reference  point  is  dependent  on  expected  vehicle  operating  depth. 
To  achieve  a  right-handed  coordinate  system,  the  filter  actually  uses  vehicle  altitude  above 
or  below  the  reference  point  rather  than  depth,  but  this  coordinate  system  change  is 
imbedded  within  the  filter  algorithm.  A  reference  depth  near  the  expected  operating  depth 
is  chosen  by  the  operator  in  the  KFSetup  program  prior  to  commencing  operations. 

The  final  reference  point  is  time.  Each  sensor  package  data  stream  contains  a  time 
stamp  with  units  of  Julian  days.  The  reference  time  for  each  day  is  the  beginning  of  the 
Julian  day.  Therefore,  the  time  value  used  in  the  filter  is  always  a  fraction  of  a  day. 

3.6.2  Initial  State  Vector  and  Error  Covariance  Matrix 

Prior  to  the  first  filter  update  step,  the  initial  slate  vector.t  and  error  covariance 
matrixP  must  be  provided.  Nonnally,  the  inifial  sensor  data  obtained  are  used  for  the  state 
vector,  and  the  predetermined/?  matrix  is  augmented  with  the  initial  calculated  variable 
elements  of/?  (for  velocimeter  and  heading  measurements)  to  provide  P .  However,  if 
some  other  more  accurate  method  is  available  for  determining  any  of  these  variables,  the 
better  value  can  be  substituted  instead. 

3.6.3  Removing  Known  Biases 

Of  crucial  importance  in  optimizing  Kalman  filter  performance  is  the  removal  of 
bias.  Uncorrected  bias  provides  a  consistent  source  of  enor  in  the  state  estimate  vector  and 
must  be  eliminated  as  much  as  possible. 

Because  of  its  use  of  doppler  to  measure  velocity,  whereby  zero  frequency  change 
equates  to  zero  velocity  in  that  direction,  the  doppler  velocimeter  is  relatively  immune  to 
bias.  Therefore,  no  attempt  is  made  to  compensate  for  it. 
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The  position  information  is  also  insensitive  to  bias,  since  the  use  of  a  reference 
point  automatically  removes  bias  from  the  system.  Once  again,  no  bias  correction  is 
needed. 

The  bias  of  the  pitch  and  roll  sensors  is  directly  related  to  the  precision  of  their 
placement  on  the  vehicle.  Normally,  the  final  installation  of  the  sensors  on  the  vehicle  is 
performed  in  the  lab,  which  allows  the  sensors  to  be  checked  to  ensure  they  are  unbiased. 
Therefore,  no  bias  adjustment  is  made  in  the  filter.  If  the  sensors  must  be  replaced  at  sea, 
accurate  measurement  of  pitch  and  roll  on  a  moving  ship  is  virtually  impossible,  so  there 
is  no  way  to  measure  bias  accurately.  In  this  case,  the  sensor  is  mounted  as  precisely  as 
possible  and  the  filter  assumes  unbiased  measurements.  Since  pitch  and  roll  are  usually 
rather  small,  the  effects  of  undetermined  bias  on  the  coordinate  transformations  is 
negligible. 

Compensation  for  gyro  bias  and  magnetic  compass  bias  is  discussed  in  Section 
3.5.3.5.  Additionally,  any  known  initial  bias  is  included  as  the  initial  condiuon  of  the  state 
variable  for  each  heading  sensor  bias.  Short-term  effects  on  magnetic  compass  bias  caused 
by  thruster  operation  cannot  be  calculated  and  are  therefore  ignored. 

As  discussed  in  Section  3.5.2,  the  biases  inherent  to  the  linear  accelerometers  and 
angular  velocimeters  must  be  determined  by  laboratory  testing.  With  the  vehicle 
motionless  and  with  zero  pitch  and  roll,  the  output  voltage  of  each  instrument  can  be 
measured.  The  voltage  caused  by  the  gravity  vector  is  calculated  so  as  not  to  be  removed 
from  the  instmment  measuring  vertical  acceleration,  but  the  other  measured  voltages  are 
subtracted  from  the  data.  These  biases  are  also  included  in  the  KFSetup  program. 


3.7  Changes  Required  to  the  Standard  Kalman  Fiher 

In  this  application,  there  are  three  major  changes  that  must  be  made  to  the  standard 
Kalman  filter  described  in  Section  3.4.  First,  due  to  the  variability  of  the  data  update  rate, 
a  constant  filter  update  frequency  is  impractical.  Second,  the  difference  in  coordinate 
systems  among  the  various  sensors  must  be  addressed.  Finally,  there  are  gaps  in  certain 
data  streams,  especially  for  X-  and  Y-pt)sition  fixes.  The  (ilter  must  be  able  to  function 
despite  these  gaps  to  provide  continuous  state  estimates. 
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3.7.1  Adjustments  Required  Due  to  Varying  Filter  Update  Rate 

3.7.1.1  Determining  the  Desired  Time  Step 

To  determine  the  desired  time  step,  competing  objectives  must  be  considered.  In 
support  of  a  longer  time  step,  there  are  two  major  arguments.  First,  adequate  time  must  be 
given  to  obtain  the  data  necessaiy  for  the  propagation  of  the  filter.  Second,  the  interval 
must  be  long  enough  to  permit  real-time  processing  of  the  data. 

There  is  only  one  counterpoint  in  support  of  a  shorter  time  step,  but  it  is  a  vital  one. 
If  the  time  step  is  too  long,  the  true  vehicle  dynamics  will  be  lost,  especially  if  significant 
maneuvering  is  occuiring.  Although  a  long  time  step  may  provide  a  reasonable 
approximation  of  average  vehicle  position  and  attitude,  the  lack  of  precise,  continuous 
information  will  degrade  any  sidescan-sonar  or  video  imaging  accuracy.  A  compromise  is 
necessary. 

Of  the  four  data  packages  described  in  Section  ."^.2,  only  two  have  update  rates  that 
are  both  consistent  and  rapid.  The  IMU  package  produces  data  at  approximately  10  Hz, 
while  the  attitude  package  operates  at  about  5  Hz.  Therefore,  the  Kalman  filter  is  designed 
with  a  minimum  requirement  of  having  data  from  these  two  packages  prior  to  performing 
a  filter  update  step.  As  an  overall  compromise,  a  time  step  of  0.5  s  is  used.  This  is  short 
enough  to  capture  the  system  dynamics,  but  long  enough  to  allow  real-lime  processing  of 
data  and  to  ensure  both  IMU  and  attitude  package  information  is  normally  available. 

To  prevent  filter  processing  error  in  the  event  of  an  inteiTuption  in  data  flow,  the 
filter  algorithm  checks  to  ensure  that  at  least  one  sample  of  each  data  package  is  available 
within  the  0.5  s  of  data  to  be  processed.  If  either  is  missing,  the  filter  continues  to  accept 
new  data  unul  both  are  present.  As  soon  as  this  condition  is  met,  the  data  obtained  from 
each  sensor  during  that  time  step  are  averaged  and  the  next  filter  update  step  is  performed. 

3.7.1.2  Predicting  the  Next  State  Vector  and  Error  Covariance  Matrix 

In  the  standard  Kalman  filter,  the  last  step  of  filter  propagation  produces  the 
predicted  values  ot'x{t  +  \\r)  and  P  {r  +  [\r)  .As  described  in  Section  3.4.2.2,  one  of  the 
inputs  required  for  the  conversion  ['vom  a  continuous  to  a  discrete  matrix  is  the  magnitude 
of  the  time  step.  For  this  filter,  that  magnitude  is  unknown.  First,  it  will  probably  not  be 
exactly  0.5  s,  since  the  actual  time  step  is  the  difference  in  time  between  the  first  and  the 
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last  data  received  prior  to  0.5  s  elapsing.  Second,  if  either  IMU  package  or  attitude 
package  data  is  not  received  during  that  first  ().5-s  interval,  the  filter  algorithm  waiLs  until 
the  missing  data  are  provided.  Thus,  the  time  interval  could  be  considerably  longer. 
To  con^ect  for  these  varying  time  steps,  a  small  change  in  filter  mechanics  is 
required.  Instead  of  computing  the  predicted  values  at  the  end  of  a  given  filter  update  step, 
the  filter  algorithm  waits  undl  the  beginning  of  the  next  filter  update  cycle.  Therefore,  the 
filter  time  step  is  known  since  the  algorithm  has  already  allowed  0.5  s  to  elapse  and  has 
then  checked  for  the  two  data  streams,  waiUng  as  necessary  to  obtain  them.  By  allowing 
the  predicted  values  to  lag  until  the  next  time  step  is  confirmed,  the  continuous-to-discrete 
conversion  can  be  made  accurately. 

3.7.2  Coordinate  System  Transformations 

Due  to  the  difference  in  coordinate  systems  among  the  various  sensors,  two 
separate  calculations  are  required.  Both  use  the  current  estimates  of  roll,  pitch,  and 
heading  as  input.  The  first  removes  the  effect  of  the  gravity  vector  on  the  linear 
accelerometer  measurements.  The  second  computes  the  currentA  matrix  used  in  the 
prediction  of  the  next  state  estimate  and  eiTor  covariance  matrix. 

3.7.2.1  Removing  the  Effect  of  the  Gravity  Vector 

To  provide  an  accurate  rellecUon  of  vehicle  linear  acceleration,  the  output  of  the 
IMU  package  must  be  modified  to  remove  the  gravity  vector  bias.  This  vector^t,'  is 
assumed  constant  with  a  value  of  9.8  m/s"  poinUng  in  the  -z  direction  in  earth-referenced 
coordinates.  The  filter  algorithm  uses  the  current  state  estimates  of  roll  and  pitch  to 
calculate  the  component  of  the  gravity  vector  included  in  the  output  of  each  of  the  three 
linear  accelerometers.  The  effect  of  the  gravity  vector  on  x-acceleration  is  shown  in  Fig. 
3-5  as  an  example.  The  calculations  are  as  follows  (({)  =  roll,a)  =  pitch,  i^  =  -9..S  ): 

Actual  x-accelerafion  =    (Measured  x-acceleration)  -t-^'sincj). 

Actual  y-acceleration  =    (Measured  y-acceleration)  -j,'sinco. 

Actual  z-acceleralion  =    (Measured  z-acceleration)  -,t,'COS(j)cos(I3. 
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Fig.  3-5.  Effect  of  gravity  vector  on  measured  x-acceleration. 

These  calculations  are  actually  approximations  because  the  effect  of  one  attitude 
on  the  measurement  of  another  is  ignored.  For  example,  as  pitch  increases  the  sensitivity 
of  the  roll  sensor  is  decreased  due  to  the  lessened  gravity  vector  component  on  the 
inclinometer's  sensitive  axis.  In  the  worst  case,  a  pitch  of  90  degrees  results  in  no 
sensitivity  to  roll.  For  typical  vehicle  operations,  during  which  pilch  and  roll  are  nomially 
less  than  10  degrees,  the  errors  introduced  by  these  approximations  are  negligible. 

3.7.2.2  Calculating  the  A  Matrix 

As  explained  in  Section  3.4.2.2,  part  of  the  prediction  step  of  the  filter  update  cycle 
converts  the  continuous  time  A  and  G  matrices  into  their  discrete  time  counterparts. 
Unlike  the  linear  filter  case,  in  this  nonlinear  filter  the  A  matrix  must  be  recalculated 
during  each  prediction  step. 

To  be  more  specific,  the  detailed  version  of  the  system  propagation  equation  is  as 
follows.  The  A  matrix  is  constant  with  the  exception  of  the  3x3  portion  identified  with 
"a  ,"  because  this  portion  of  the  A  matrix  propagates  the  change  in  vehicle  position  due  to 
the  previously  estimated  vehicle  velocities.  Since  vehicle  velocities  are  body-referenced 
and  positions  are  earth-referenced,  a  coordinate  transformation  is  necessary. 

The  current  heading  estimate,  which  is  maintained  in  the  traditional  clockwise- 
positive  system  used  by  the  compass  and  gyro,  is  altered  to  a  counterclockwise-positive 
yaw  angle  to  maintain  a  right-handed  coordinate  system.  With  units  of  radians,  this  yaw 
angle  is  calculated  by: 
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K  =  2k-  (CuiTciil  Heading  Estimate) . 

The  3x3  nonzero  matrix  within  the  A  matrix  is  computed  as  lollows.  Required  inputs  are 
'the  cuirent  state  estimates  ol'  roll  (0),  pitch  (co),  and  heading  (k). 

Element  (1,1)  =  cos(t)cosK 

Element  (1,2)  =    sin O) sin (j) cos k-  coswsinK 

Element  (1,3)  =  cosa)sin(j)C()SK  +  sincosinK 

Element  (2,1)  =  sinKcosc]) 

Element  (2,2)  =  sino)sin(|)sinK  +  cos  O) cos  k 

Element  (2,3)  =  cosO)sin(j)sinK-  sinwcosK 

Element  (3,1)  =  -sincf) 

Element  (3,2)  =  sinocosc]) 

Element  (3,3)  =  coscocoscj) 
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3.7.3  Varying  the  C  Matrix  with  the  Available  Data 

Section  3.7.1.1  describes  the  process  by  which  the  filter  ensures  it  has  both  IMU 
and  attitude  infomiation  available  prior  to  performing  an  update  step.  During  this  period 
of  accepting  new  data,  doppler  velocimeter  and/or  X-Y  position  information  may  also  be 
obtained.  To  perform  the  update  step  correctly  in  light  of  the  available  data,  the  filter 
algorithm  keeps  track  of  which  data  are  available  prior  to  performing  the  update  operation. 
After  averaging  all  data  from  each  sensor,  the  C  matrix  must  be  computed  for  use  by  the 
filter.  As  part  of  this  computation,  the  measurement  vector^;  is  also  determined  based  on 
which  sets  of  data  have  been  obtained. 

After  the  first  four  position  fixes  have  been  obtained  (and  therefore  true  heading 
infomiation  is  available  with  each  subsequent  fix),  there  are  four  possibilities  for  the  C 
matrix  and  measurement  vector: 

1.    Only  IMU  package  and  attitude  package  data  are  available.  The  measurement  vec- 
tor is: 


x-accelerafion 

y-acceleration 

z-acceleration 

z-position 

roll  rate 

pitch  rate 

yaw  rate 

roll 

pilch 

heading  by  gyro 

heading  by  compas.s 


and  the  corresponding  C  matrix  is: 
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reflecting  the  position  in  the  stale  estimate  vector.^  of  x-acceleration  (xl),  y-accel- 
eration  (x2),  z-acceleration  (x3),  z-position  (x9),  roll  rate  (xlO),  pitch  rate  (xll), 
yaw  rate  (xl2),  roll  (xl3),  pitch  (xl4),  heading  (xl5),  gyro  bias  (xl6),  and  com- 
pass bias  (xl7). 

2.    Both  attitude  package  and  XY  position  information  are  available  but  no  doppler 
velocimeter  data.  The  measurement  vector  is: 
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and  the  corresponding  C  matnx  is: 
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3.    Both  attitude  package  and  doppler  velocimeter  data  are  available  but  no  XY  posi- 
tion infoiTnation.  The  measurement  vector  is: 


x-acceleration 

y-acceleration 

z-acceleration 

x-velocity 

y-velocily 

z-velocity 

z-posilion 

roll  rate 

pilch  rate 

yaw  rate 

roll 

pitch 

heading  by  gyro 

heading  by  compass 


and  the  corresponding  C  matrix  is: 


50 


c  = 


1  0  {)  ( 
()!()( 
()()!( 
0  0  0 
0  0  0  ( 
0  0  0  ( 
0  0  0  ( 
0  0  0  ( 
0  0  0  ( 
0  0  0  ( 
0  0  0  ( 
0  0  0  ( 
0  0  0  ( 
0  0  0  { 


)  0  ( 

)  0  ( 

)  0  ( 

)  0  ( 

1  0  ( 

)  1  ( 

)  0  ( 

)  0  ( 

)  0  ( 

)  0  ( 

)  0  ( 

)  0  ( 

)  0  ( 

)  0  ( 


)  0  0  0  0  0  0  0 

)  0  0  0  0  0  0  0 

)  0  0  0  0  0  0  0 

)  0  0  0  0  0  0  0 

)  0  0  0  0  0  0  0 

)  0  0  0  0  0  0  0 

)!()()  0  0  0  0 

)()!()  0  {)  0  0 

)()()!  0  0  0  0 

)  0  0  0  1  0  0  0 

)  0  0  0  0  1  0  0 

)  0  0  0  {)  0  1  {) 

)  0  0  0  0  0  0  1 
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4.    The  final  case  is  when  data  are  available  from  all  senso 
uses  the  same  construction  as  the  state  vector. 


•s.  The  measurement  vector 


x-acceleration 

y-acceleration 

z-acceleration 

x-velocity 

y-velocity 

z-velocity 

x-position 

y-position 

z-posilion 

roll  rate 

pitch  rate 

yaw  rate 

roll 

pitch 

heading  by  gyro 

heading  by  compass 

true  heading 


while  the  C  matrix  is: 
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-1 

-1 

()_ 

By  using  this  varying  C  matiix,  the  Kahnan  (ilter  mechanics  remain  the  same  for 
each  update  step  regardless  of  the  available  data.  Additionally,  the  error  covariance  matrix 
automatically  reflects  the  data  available.  For  each  filter  update  step,  each  state  variable's 
error  covariance  is  increased  due  to  elapsed  time,  and  then  decreased  if  a  measurement  of 
that  state  variable  is  available.  If  a  given  measurement  is  not  available,  the  corresponding 
state  estimate  has  a  greater  error  covariance  than  before.  Similarly,  available  data  reduces 
the  error  covariance  appropriately  based  on  the  assigned  standard  deviation  oi'  that 
particular  sensor  output. 

3.7.4  Incorporating  Fading  Memory 

One  problem  with  the  standard  Kalman  implementation  is  filter  "laziness".  In  a 
completely  observable  system,  the  Kalman  gain  approaches  zero  as  time  goes  to  infinity. 
Therefore,  the  filter  ignores  new  data  since  it  believes  that  it  has  essentially  perfect 
knowledge  of  all  state  variables.  Changes  in  those  variables  caused  by  vehicle  operations, 
such  as  a  change  in  course  or  velocity,  are  not  reflected  in  the  filter. 
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Gelb  provides  a  simple  solution  to  this  problem  using  recursive  fading  memory 
filtering.  To  force  the  filter  to  continue  using  new  data,  the  enor  covariance  of  the 
predicted  state  vector  is  artificially  increased  {Gelh,  1974).  This  is  accomplished  with  a 
simple  addition  to  the  algorithm  for  calculating  P  (t  +  l\t)  .  Now,  the  equation  is: 

Pit+[\t)   =  sX(^xP{t\r)  x<^'  +  rxQxr', 

with  the  only  change  being  the  addition  of  the  s  term,  which  is  the  fade  factor.  This  is 
calculated  by 

.V  =  exp  (Ar/x)  , 

where 

At  is  the  measurement  interval  (0.5  s), 
T  is  the  age-weighting  time  constant. 

For  this  filter,  the  time  constant  is  chosen  to  he  30  s  to  ensure  a  (ilter  responsive  to  rapid 
changes  in  vehicle  parameters  while  still  retaining  sufficient  memory.  The  resulting  fade 
factor  is  1.0168. 

As  Sorenson  and  Sacks  explain  {Sorenson  and  Sacks,  1971),  the  use  of  this  fade 
factor  does  not  affect  the  stability  of  the  Kalman  filter.  The  only  disadvantage  is  that  by 
using  the  fade  factor,  the  error  covariance  matrix  is  altered.  Therefore,  the  eiTor 
covariance  matrix  is  no  longer  the  best  estimate  of  the  error  in  the  state  estimate  but  only 
an  approximation. 


3.8  Observability 

If  a  system  is  completely  observable,  the  state  vector  can  be  entirely  determined 
from  the  measurements.  If  the  system  is  only  partially  observable,  then  any  system  errors 
involving  unobservable  quanuties  that  propagate  through  the  filter  cannot  be  identified  by 
measurements.  For  example,  if  position  is  not  observed,  then  any  consistent  eiTors  in 
velocity  measurements  will  continue  to  increase  the  position  error.  In  contrast,  if  velocity 
is  not  measured  but  position  is,  velocity  can  still  be  observed  due  to  its  known  effect  on 
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position.  In  the  context  of  vehicle  navigation,  a  partially  observable  system  means  that 
there  is  no  way  to  provide  a  reasonable  estimate  of  the  unobservable  quantities.  If  that  is 
the  case,  the  filter  will  be  useless. 

To  be  observable,  the  matrix  [C'^\A'V\(A'^)-Ch  .  MA'^T^C'^]  must  have  rank  n 
{Gelb,  1974).  When  all  data  are  available,  the  C"^  matrix  has  17  linearly  independent 
columns' by  inspection,  and  the  system  is  trivially  observable.  However,  when  fewer  data 
are  available  the  observability  changes.  When  only  IMU  package,  attitude  package,  and 
doppler  velocimeter  information  is  available,  the  observability  matrix  is  no  longer  trivial. 
Powers  of  A    greater  than  two  yield  only  zero  matrices  and  do  not  contribute  to 
observability.  Eliminating  zero  columns  and  repeated  columns. 


[d\A^d\A^-d]  = 


[  0  0  {)  0  0  0  0  0  0  0  0 

)!()()  0  {)  0  0  0  0  0  0 

)()!()  0  0  0  {)  0  0  0  0 

)()()!  0  0  0  0  0  0  0  0 

)  0  0  0  1  0  0  0  0  0  0  0 

)  0  0  0  0   1  0  0  0  0  0  0 

)  0  0  0  0  0  0  0  0  0  0  0 

)  0  0  0  0  0  0  0  0  0  0  0 

)  0  0  0  0  0  1  0  0  0  0  0 

)  0  {)  0  0  0  0  1  0  0  0  0 

)  0  0  0  0  {)  0  0  1  0  {)  0 

)  0  0  0  0  0  0  {)  0  1  0  0 

)  0  0  0  0  0  0  0  0  0  1  0 

)  0  0  0  0  0  0  0  0  0  0   1 

)  0  0  0  {)  0  0  0  {)  0  0  0 

1 

)  0  0  0  0  {)  0  0  0  0  0  0 

-1 

)  0  0  0  0  0  0  0  0  0  {)  0 

0 

0  Y 

0  A 

0  5 

a  0 

(3  0 

r  {) 

0 

-1 

The  Gre^k  lettersa,  p,  F,  y,  A,  and  5  represent  elements  of  the  3x3  coordinate 
transformadon  matrix.  In  this  case,  they  do  not  add  to  the  rank  of  the  matrix  since  column 
15  containing  them  is  simply  a  linear  combination  of  columns  four  through  six,  while 
column  16  is  a  linear  combination  of  the  lirst  three  columns.  Therefore,  the  rank  of  the 
matrix  is  only  14,  leaving  three  state  variables  as  unobservable.  Not  surprisingly,  two  of 
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the  unobservable  states  in  this  case  are  X-  and  Y-positions.  Without  observing  position, 
there  is  no  way  to  deteirnine  if  the  estimates  of  velocity  and  attitude  contain  errors  causing 
divergence  of  the  state  estimates  from  the  actual  values.  The  third  unobservable  state 
concerns  gyro  and  compass  bias.  Without  position  measurements,  true  heading 
measurements  are  impossible.  While  independent  measurements  of  gyro  and  magnetic 
compass  headings  allow  the  filter  to  observe  the  difference  between  the  two  biases, 
without  true  heading  measurements  the  actual  value  of  either  cannot  be  determined  (since 
the  difference  can  be  calculated,  knowing  one  of  these  biases  would  permit  calculation  of 
the  other.  Thus  there  is  only  one  additional  unobservable  state  due  to  the  lack  of  true 
heading  information). 

Similar  calculations  can  be  performed  for  the  other  two  possible  data 
combinations.  Lacking  only  velocity  data,  the  system  is  observable  since  position 
measurements  can  verify  the  accuracy  of  velocity  estimates.  Without  X-  and  Y-position 
information  available  at  least  occasionally,  the  filter  cannot  be  expected  to  provide  valid 
estimates. 
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Chapter  4 

Kalman  Filter  Performance  Using 
Simulated  Data 


4.1  Purpose  of  Simulation 

The  initial  tests  of  this  Kahnan  filter  are  performed  using  simulated  data.  This 
allows  simple  modification  of  data  parameters  to  evaluate  filter  performance  under  a 
variety  of  conditions.  Several  tests  are  perfonned  using  the  same  basic  data  set.  The 
specific  parameters  for  each  test  are  explained  in  detail. 

4.2  Data  Generation 

4.2.1  Method  of  Data  Generation 

The  total  time  of  the  simulation  is  fixed  at  one  hour.  This  provides  ample  time  for 
filter  evaluation  and  coincides  with  the  period  used  for  the  actual  data  evaluation 
conducted  in  Chapter  5. 

Generating  a  valid  data  set  is  a  multistep  process.  First,  the  control  inputs  are 
determined  using  reasonable  values  for  linear  accelerations  and  angular  velocities.  Then 
the  nominal  standard  deviations  for  the  control  input  measurements  and  system 
measurements  ai'e  computed,  along  with  system  initial  conditions.  These  use  the  values 
determined  in  Section  3.5,  unless  otherwise  noted.  For  all  simulated  data,  initial 
conditions  are  found  in  Table  4-1. 

To  generate  noise-free  measurements  in  the  appropriate  coordinate  system  for  each 
sensor,  first  the  attitude  measurements  are  computed  for  the  entire  hour  using  the 
MATLAB  function  "dlsim,"  with  the  angular  velocity  measurements  as  input.  Using  these 
noise-free  attitude  computations,  the  vehicle-referenced  linear  accelerations  are 
transfonned  into  eanh-referenced  coordinates  using  the  coordinate  transformation  found 
in  Section  3.7.2.2.  Next,  "dlsim"  is  used  again  to  provide  earth-referenced  velocities  and 
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Table  4-1:  Initial  Conditions  for  Simulations 


Parameter 

Initial  Value 

X-Velocity 

0 

Y-Velocity 

0.5  m/s 

Z- Velocity 

0 

X-Position 

0 

Y-Position 

0 

Z-Positioii 

0 

Roll 

0 

Pitch 

0 

Heading 

300° 

Gyro  Bias 

0 

Compass  Bias 

-18° 

positions.  Finally,  attitude  is  used  to  transfonn  the  computed  velocities  into  the  vehicle- 
referenced  coordinates  used  in  the  filter. 

After  these  noise-free  measurements  are  computed,  noise  is  added  using  the 
MATLAB  function  "randn,''  which  provides  a  Gaussian  distribution  with  the  desired 
vai'iance.  Additionally,  the  frequency  of  data  output  from  each  sensor  is  determined.  The 
last  step  is  to  add  desired  gyro  bias,  compass  bias,  and  any  gyro  drift. 

4.2.2  Noise-free  Data  Used  for  All  Simulation  Tests 

To  provide  a  convenient  basis  for  comparison  among  the  tests,  the  same  noise-free 
data  set  is  used  for  each.  Figures  4- 1  through  4-4  provide  a  graphic  display  of  a  sample  of 
these  measurements. 
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Fig.  4-1.  Noise-free  measurement  of  Y- velocity. 
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Fig.  4.2.  Noise-free  measurement  of  Y-position. 
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Fig.  4-3.  Noise-free  measurement  of  vehicle  pitch. 
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Fig.  4-4.  Noise-free  measurement  of  vehicle  heading  by  gyro. 
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Fig.  4-5.  Comparison  of  measured  and  estimated  X-Y  position. 


4.3  Simulation  Tests 

4.3.1  Test  1:  Accurate  Initial  Conditions 

For  the  first  test,  doppler  velocimeter  readings  are  provided  every  10  s  and  X-Y 
position  information  every  100  s.  The  initial  conditions  provided  for  the  state  variables 
match  their  actual  values,  including  gyro  and  magnetic  compass  biases.  Additionally,  a 
gyro  drift  of  5°/lir  is  simulated. 

Figure  4-5  shows  a  comparison  of  actual  to  estimated  position.  Position  data  are 
available  frequently,  allowing  the  filter  to  maintain  a  close  match  between  estimated  and 
actual  positions.  Figure  4-6  provides  a  close-up  of  a  smaller  part  of  the  data  to  allow  better 
resolution. 

Estimating  parameters  with  frequent  data  updates  is  relatively  easy  for  the  filter  to 
accomplish.  For  example.  Fig.  4-7  shows  a  portion  of  the  data  set  comparing  actual  and 
estimated  Z-position;  Fig.  4-8  shows  the  same  for  roll.  Heading  is  a  more  difficult  state  to 
estimate  due  to  the  inherent  biases  of  the  available  instruments,  the  gyro  and  the  compass. 
Figure  4-9  shows  the  estimate  of  heading  throughout  the  data  mn;  Fig.  4-10  shows  a 
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Fig.  4-6.  Close-up  of  X-Y  Position  Comparison. 
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Fig.  4-7.  Comparison  of  actual  and  estimated  Z-position. 
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Fig.  4-8.  Comparison  of  actual  and  estimated  roll. 
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Fig.  4-9.  Comparison  of  actual  and  estimated  heading. 
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comparison  of  actual  and  estimated  biases  for  the  gyro.  With  the  frequent  availability  of 
data,  the  filter  provides  a  good  estimate  of  the  heading  and  associated  biases.  Overall,  with 
Gaussian  noise  and  frequent  data  the  filter  performs  well.  Subsequent  tests  check  its 
performance  under  more  strenuous  conditions. 

4.3.2  Test  2:  Inaccurate  Initial  Conditions  Combined  with  Position  Data 
Gap 

For  the  second  test,  all  parameters  are  the  same  except  for  those  noted  in  Table  4-2. 
Additionally,  no  position  information  is  available  for  a  50()-s  interval  towards  the  start  of 
the  run.. 

Table  4,2:  Initial  Conditions  for  Simulation  Test  2. 


Parameter 

Initial  Estimate 

Actual  Value 

Compass  Bias 

-24" 

-18" 

Gyro  Bias 

-7" 

0" 

Heading 

307" 

300" 
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Fig.  4-11.  Comparison  of  measured  and  estimated  X-Y  positions. 

As  can  be  seen  in  Fig.  4-11,  the  lack  of  early  position  eiTor  combined  with  the 
incorrect  initial  heading  and  biases  causes  some  inaccuracies  during  the  time  without 
position  data.  However,  after  position  infomiation  is  again  received  the  filter  quickly 
adjusts  to  correct  itself. 

The  large  error  in  the  initial  heading  estimate  is  somewhat  artificial  since  in  actual 
operation,  the  towed  vehicle  should  normally  be  within  a  few  degrees  of  the  surface  ship's 
average  heading.  However,  even  with  a  large  initial  eiTor  heading,  gyro  bias,  and  compass 
bias  all  approach  their  actual  values  by  the  end  of  the  hour,  as  seen  in  Figs.  4-12  through 
4-14.  If  position  infomiation  had  been  available  throughout  the  data  run,  the  final 
estimates  would  have  been  even  closer. 

4.3.3  Test  3:  Estimation  Without  Doppler  Velocimeter 

The  last  test  uses  exactly  the  same  parameters  as  test  2,  except  that  no  doppler 
velocimeter  information  is  included.  Under  these  conditions,  an  estimate  of  the  ^ 
importance  of  the  velocimeter  can  be  made.  The  same  graphs  are  produced  for 
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Fig.  4-12.  Heading  estimate. 
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Fig.  4-13.  Estimated  and  actual  gyro  bias. 
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Fig.  4-14.  Estimate  of  magnetic  compass  bias. 


comparison.  As  can  be  seen  in  Figure  4-15,  the  gap  in  position  data  combined  with  the 
lack  of  accurate  velocimeter  information  creates  such  a  large  error  in  position  due  to 
velocity  errors  that  all  subsequent  fixes  are  rejected  by  the  filter  algorithm.  This  renders 
position  unobservable,  and  filter  divergence  results.  However,  since  the  compass  and  gyro 
are  still  producing  independent  sources  of  heading  information,  the  filter  is  able  to  make 
some  corrections  to  bias.  Without  fixes  to  provide  true  heading  measurements,  the  heading 
does  not  converge  to  its  actual  value  but  uses  a  weighted  average  of  the  gyro  and  compass 
measurements  to  minimize  the  error. 
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Fig.  4-15.  Comparison  of  measured  and  estimated  X-Y  position. 
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Fig.  4-16.  Estimated  heading. 
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Fig.  4-17.  Estimated  magnetic  compass  bias. 
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Fig.  4-18.  Comparison  of  actual  and  estimated  gyro  bias. 
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Chapter  5 

Kalman  Filter  Performance  Using  Actual 
Data 


5.1  Description  of  Data 

The  data  used  for  this  evaluation  were  obtained  in  July,  1994,  during  operation  of 
the  DSL- 120,  a  towed  vehicle  mounting  a  sidescan  sonar.  Both  data  set.s  used  for  filter 
testing  comprise  approximately  one  hour  of  navigational  sensor  information.  Due  to  the 
specific  characteristics  of  the  data  package,  two  modifications  were  made  to  the  Kalman 
filter  algorithm  to  allow  proper  processing. 

First,  gyro  data  were  not  obtained.  Therefore,  only  the  magnetic  compass  is  used 
as  the  heading  sensor  for  this  data.  As  a  consequence,  the  order  of  the  filter  is  reduced  by 
one  due  to  the  removal  of  gyro  bias  as  a  state  variable. 

Second,  analysis  of  the  data  reveals  some  anomalies.  During  the  first  data  run, 
numerous  spurious  velocities  of  approximately  ."^O  m/s  are  observed.  In  the  second  data 
run,  regular  spurious  compass  readings  of  exactly  40'^  are  found.  These  anomalies  are 
removed  before  using  the  data  in  the  filter. 


5.2  First  Data  Set 

On  initial  inspection,  the  first  data  set  appears  to  be  excellent  for  Kalman  filter  use 
due  to  the  frequency  of  obtained  data,  especially  from  the  long-baseline  positioning 
system.  However,  closer  analysis  reveals  that  the  behavior  of  the  linear  accelerometers  is 
erratic.  Figure  5-1  shows  an  example  of  this  behavior.  The  difference  between  the  two 
concentrations  of  outputs  is  O.OX  g,  which  coiresponds  to  a  linear  acceleration  of 
approximately  0.8  m/s".  If  the  filter  uses  this  erratic  accelerometer  data,  it  rapidly  diverges 
due  to  the  large  estimated  accelerations  translating  into  large,  nonexistent  velocities. 
Therefore,  to  run  the  filter  on  this  data  set  the  accelerometer  measurements  are  set  to  zero 
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Fig.  5. 1.  Output  of  Linear  Accelerometer. 

for  the  entire  run.  The  Kalman  filter  uses  the  remaining  measurements  of  velocity, 
position,  roll,  pitch,  and  heading  to  estimate  all  parameters. 

Figures  5-2  and  5-3  show  the  difference  in  measured  and  estimated  X-Y  positions. 
The  doppler  velocimeter  provides  accurate  measurements  of  velocities,  which 
compensates  for  the  lack  of  accelerometer  measurements.  Therefore,  the  filter  is  able  to 
track  position  well. 

As  an  example  of  a  parameter  which  has  the  advantage  of  continuous 
measurements,  a  comparison  of  measured  and  estimated  roll  is  shown  in  Fig.  5-4.  The 
discrete  character  of  the  sensor  measurements  compared  to  the  continuous  Kalman  filter 
estimate  is  obvious. 

The  last  estimate  meriting  special  attention  is  heading.  With  the  frequent  fixes,  true 
heading  measurements  are  readily  available.  As  can  be  seen  from  Figs.  5-5  and  5-6, 
occasionally  an  maccurate  true  heading  measurement  will  temporarily  affect  the  estimated 
heading  and  bias. 
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Fig.  5.2.  Comparison  oT  estimated  and  actual  X-Y  positions. 
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Fig.  5.3.  Close-up  of  X-Y  position  comparison. 
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Fig.  5.4.  Comparison  of  measured  and  estimated  roll. 
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Fig.  5.5.  Estimated  heading. 
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Fig.  5.6.  Estimated  compass  bias. 


5.3  Second  Data  Set 

The  second  data  set  poses  more  of  a  challenge  for  the  filter,  since  after  four  initial 
fixes  are  obtained  there  is  a  gap  of  nearly  one-half  hour  without  further  XY-position 
information.  However,  the  linear  accelerometers  do  not  show  the  same  erratic  behavior 
during  this  data  set  and  are  therefore  used  as  part  of  the  measurement  vector.  Figures  5-7 
through  5-12  show  the  performance  of  the  filter  in  estimating  position  in  light  of  this  data 
gap.  When  position  infonnation  is  restored,  there  is  a  discontinuity  in  the  filtered  estimate 
of  position  due  to  the  eiTor  developed  by  the  filter  estimate  during  the  gap.  After  frequent 
fix  informadon  is  again  received,  filter  position  estimates  track  with  actual  positions 
obtained  from  the  long-baseline  navigation  system. 

Besides  position,  the  most  important  vehicle  parameter  to  estimate  accurately  is 
heading.  Without  an  accurate  heading,  sidescan  sonar  information  will  be  inaccurate  as 
well.  Since  both  the  gyro  and  the  compass  can  be  biased,  a  vital  characteristic  of  the  filter 
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Fig.  5.7.  XY-position  estimates  (:)  compared  to  position  fixes  (o).  Note  the  presence  of  a 
flyer  in  the  upper  left  corner,  which  was  properly  rejected  by  the  filter. 


is  its  ability  to  estimate  heading.  Figure  5-112  shows  the  estimated  heading  during  the 
hour.  To  understand  filter  behavior  in  the  vicinity  of  this  disconfinuity,  X-position 
behavior  is  examined  individually  in  Fig.  5-10.  After  a  fix  is  received  following  the  long 
data  gap,  the  first  fix  is  weighted  heavily  and  the  estimated  X-position  jumps  to  the 
location  of  the  fix.  As  can  be  seen  in  the  figure,  however,  that  first  fix  was  rather 
inaccurate,  although  still  within  the  specification  of  the  fix  rejection  filter.  Subsequent 
fixes  are  weighted  less  heavily  since  the  emir  covariance  matrix  is  adjusted  after  receipt  of 
the  first  fix.  Therefore,  estimated  position  adjusts  to  actual  position  over  several  fixes. 

The  jump  in  esUmated  heading  at  t=2()()()  seconds  is  a  result  of  the  gap  in  position 
information.  However,  despite  this  lengthy  gap,  the  estimated  heading  remained 
consistent  within  three  degrees. 

Parameters  with  more  frequent  data  update  rates  are  easier  for  the  filter  to  estimate 
since  measurements  are  confinuously  matched  against  the  estimates.  As  one  example,  the 
comparison  of  esfimated  Z-position  for  a  small  portion  of  the  hour  is  shown  in  Figure  5- 
12. 
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Fig.  5.8.  Comparison  of  estimated  and  actual  vehicle  track  (blowup  of  Figure  5-7  with  the 

region  of  the  llyer  excluded). 
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Fig.  5.9.  Close-up  of  region  of  discontinuity  following  gap  in  position  information. 
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Fig.  5.10.  X-position  behavior  in  the  vicinity  of  the  discontinuity. 
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Fig.  5.11.  Estimated  Heading. 
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Fig.  5.12.  Comparison  of  measured  and  estimated  Z-position. 

With  the  aid  of  the  doppler  velocimeter,  the  Kalman  filter  is  able  to  maintain  a 
valid  estimate  of  vehicle  parameters  during  this  lengthy  gap  in  position  information.  The 
key  requirements  in  achieving  this  are: 

1.  The  doppler  velocimeter  is  available  to  provide  accurate  velocity  infoiTnation 
throughout  the  data  gap. 

2.  Prior  to  losing  position  information,  heading  accuracy  was  sufficient  to  allow  the 
Kalman  filter  to  bridge  the  gap  while  still  remaining  within  the  window  for  fix 
acceptance  once  position  data  is  regained. 
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Chapter  6 

Conclusions 


6.1  Fulfillment  of  Objectives 

The  objective  of  this  thesis  was  to  develop  an  extended  Kalman  filter  that  could 
provide  accurate  estimates  of  underwater  vehicle  position  and  attitude  in  real-time.  Based 
on  the  tests  of  the  filter  using  both  simulated  and  actual  data,  I  believe  that  the  result  is  a 
qualified  success.  On  the  positive  side,  the  Kalman  filter  was  able  to  process  the  data  fast 
enough  to  permit  real-time  processing  onboard  ship,  which  would  allow  major  reductions 
in  post-cruise  processing  costs.  Additionally,  the  real-time  processing  allows  those 
conducting  sidescan  surveys  to  verify  that  the  desired  areas  have  been  mapped  by 
checking  the  navigational  estimates. 

A  second  success  is  that  when  provided  with  sufficiently  accurate  and  frequent 
data,  the  Kalman  filter  can  provide  accurate  estimates  of  all  vehicle  motion  and  attitude 
changes.  Even  when  gaps  occur  in  the  positioning  system,  the  accuracy  of  the  doppler 
velocimeter  combined  with  a  good  prior  estimate  of  heading  allows  the  filter  to  maintain 
sufficient  accuracy  to  arrive  close  to  measured  position  when  position  data  is  regained. 
Also,  the  Kalman  filter  has  the  ability  to  estimate  gyro  bias  and  compass  bias  to 
compensate  for  inaccurate  initial  heading  infoiTnadon  and  the  effects  of  gyro  drift. 
Therefore,  the  heading  estimate  becomes  increasingly  accurate  as  fix  information  is 
received  and  the  filter  is  allowed  to  update  over  longer  periods  of  time. 

In  contrast  to  the  successes,  there  were  also  some  failures.  Many  of  these  can  be 
attributed  to  sensor  limitations,  especially  those  of  the  linear  accelerometers  contained 
within  the  IMU  package.  A  constant  bias  can  be  compensated  for,  but  in  the  first  data  run 
there  was  an  example  of  erratic  accelerometer  behavior  that  threatened  the  viability  of  the 
entire  estimation  process.  Only  by  recognizing  this  problem  and  relying  on  the  doppler 
velocimeter  entirely  could  accurate  estimates  of  vehicle  position  be  maintained.  While  in 
principle  the  Kalman  filter  is  designed  to  permit  any  measurement  to  be  used  despite  its 
noise  characterisfics,  this  application  tests  that  theory  due  to  the  vaiying  data  rates  of  the 
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sensors.  For  a  "standard"  Kalman  filler,  all  measurements  are  obtained  at  every  lilter 
update  step.  Therefore,  noisy  measurements  can  be  made  nearly  meaningless  by  assigning 
greater  inherent  accuracy  to  less  noisy  ones.  In  this  case,  that  is  not  possible.  When  the 
only  available  measurement  in  the  series  of  integrations  leading  to  position  is  acceleration, 
the  filter  is  forced  to  use  it.  Even  though  its  weight  is  initially  low,  over  time  these 
measurements  have  a  significant  effect  on  the  estimates  of  velocity  and  position  until  other 
sensor  data  are  received. 

A  second  concern  is  whether  the  heading  estimates  provided  by  the  filter  are 
accurate  enough  to  be  truly  useful  for  sidescan  sonar  mapping  operaUon.  When  scanning 
at  long  range,  even  a  few  tenths  of  a  degree  of  heading  eiTor  can  be  significant.  While 
heading  accuracy  should  continue  to  improve  as  the  filter  operates  for  longer  periods,  a 
compromise  is  always  necessary  when  deciding  how  to  weight  old  measurements.  If  old 
measurements  are  weighted  too  heavily,  the  filter  does  not  respond  to  actual  changes  in 
state  variables,  such  as  a  change  in  course.  On  the  other  hand,  weighting  old 
measurements  less  results  in  greater  filter  movement  toward  new  measurements  with 
subsequent  instability  and  potentially  eiToneous  changes  in  bias  estimates. 

Finally,  the  filter  as  presently  constructed  is  of  limited  utility  in  estimating  vehicle 
motion  for  a  maneuvering  vehicle.  While,  theoretically,  control  inputs  could  be  added 
fairly  easily,  the  modeling  necessary  for  accurate  results  is  not  yet  a  reality.  While  most 
current  operafions  involving  a  thruster-controlled  vehicle  are  limited  to  a  small  area  and 
can  therefore  take  advantage  of  a  high  update  rate  for  position  information,  future 
advances  in  vehicle  technology  promise  a  long-range,  independently-operating 
autonomous  vehicle.  As  constructed  here,  it  is  doubtful  that  the  Kalman  filter  can  provide 
sufficient  accuracy  given  the  sensors  available. 


6.2  Future  Work 

There  is  much  work  that  could  be  done  to  improve  the  performance  of  this  Kalman 
filter  estimation  process.  A  few  possibilities  are  as  follows. 

First  and  most  obviously,  improved  sensors  would  translate  directly  into  improved 
measurements.  The  primary  candidates  for  such  improvements  are  the  linear 
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accelerometers  and  the  gyro.  Without  gyro  stabilization,  it  is  difficult  to  improve 
accelerometer  stability.  However,  improvements  continue  to  be  made,  and  it  is  hoped  that 
cost,  weight,  and  power  requirements  will  continue  to  decline  to  allow  better  acceleration 
measurements.  As  an  adjunct  to  this  effort,  more  testing  needs  to  be  performed  on  the 
accelerometers  in  use  to  understand  their  characteristics  and  limitations  more  fully. 
Hopefully,  the  ring-laser  gyro  will  soon  become  inexpensive  enough  for  practical  use  in 
this  application  to  improve  heading  measurements.  Also,  the  method  of  obtaining  true 
heading  used  here  is  only  one  possibility.  Any  improvements  in  true  heading  accuracy 
would  improve  the  estimates  of  both  heading  and  heading  sensor  bias. 

To  allow  this  filter  to  be  useful  for  such  highly  maneuverable  vehicles  as  Jason, 
further  work  needs  to  be  cairied  out  on  modeling  control  inputs  to  the  filter.  Additionally, 
if  a  strapdown  IMU  package  is  still  in  use,  the  addition  of  the  effects  of  angular  velocities 
on  linear  acceleration  and  velocities  may  have  to  be  considered  due  to  the  greater  angular 
velocities  involved. 

Finally,  there  are  always  refinements  which  can  be  made  to  improve  filter 
performance.  One  possibility  is  to  add  to  the  state  vector  esfimates  for  other  sensor  biases. 
This  has  the  potential  for  improving  all  measurements,  especially  those  of  the 
accelerometers,  which  appear  to  have  biases  that  are  always  present  and  frequently 
changing. 
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Appendix 

KFSetup  Program 


%  This  program  provides  the  initial  conditions  required  to  run  the  Kalman 
%  filter  algorithm. 

DTOR  =  pi/18();%  Conversion  of  degrees  to  radians 

%  The  A  matrix  propagates  the  state  variables 
A  =  zeros(17); 
A(4:6,l:3)=eye(3); 
A(13:15,10:12)=eye(3); 

%  The  G  matrix  provides  the  effects  of  inputs  (accelerations  and  angular 
%  velocities)  on  the  state  variables 

G  =  zeros(17,6); 
G(l:3,l:3)  =  eye(3); 
G(l():12,4:6)  =  eye(3); 

%  The  C  matrix  is  the  measurement  matrix 

CHDG  =  [1  -1  ();1  0  -1;1  0  ()];  %  3x3  matrix  used  to  forni  heading  portions  of  C  matrices 

CF  =  eye(17);%  Full  C  matrix  when  all  info  available 

CF(15:17,15:17)  =  CHDG; 

CNV  =  zeros(14,17);%  C  matrix  when  no  velocity  info  available 

CNV(l:3,l:3)  =  eye(3); 

CNV(4:ll,7:14)=eye(8); 

CNV(I2:14,15:17)  =  CHDG; 

CNP  =  zeros(14,17);%  C  matrix  when  no  position  info  available 

CNP(l:6,l:6)  =  eye(6); 

CNP(7:I2,9:14)  =  eye(6); 

CNP(13:14,15:17)  =  CHDG(1:2,:); 

CNPV  =  zeros(l  1,17);%  C  matrix  when  neither  position  nor  velocity  available 

CNPV(l:3,l:3)  =  eye(3); 

CNPV(4:9,9:14)=eye(6); 

CNPV(1():1 1,15:17)  =  CHDG(1:2,:); 

%  The  D  matrix  is  necessary  only  for  generating  simulated  data 

D  =  zeros(17,6); 

DNV  =  zeros(14,6); 

DNP  =  zeros(14,6); 

DNPV  =  zeros(ll,6); 

%  EiTor  Covariance  Matrix  for  random  forcing  function 
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Q  =  zeros (6); 

QXYZ  =  (().2*eye(3)).^2;  %  Set  variance  for  accelerometers 
Q(1:3,1:3)  =  QXYZ; 

QRPH  =  (2*DTOR*eye(3)).^2;     %  Set  variance  for  angular  velocimeters 
Q(4:6,4:6)  =  QRPH; 

%  Error  Covariance  Matrix  for  measurements 
RACC  =  (().l*eye(3)).^2; 
RAR  =  (().5*DTOR*eye(3)).^2; 

RPos  =  (3*eye(2)).^2;  %  Set  variance  for  acoustic  position  fixes 
RDepth  =  ().1'^2;%  Set  variance  for  deptii 

RPR  =  (().5*DTOR*eye(2)).'^2;  %  Set  variance  for  pitch  and  roll 
RHG  =  (().l*DTOR)'^2;%  Set  variance  for  heading  by  gyro 
RHM  =  (.5*DTOR)^2;      %  Set  variance  for  heading  by  compass 

%  Reference  Points 
xinit  =  0; 
yinit  =  0; 
tinit  =  0; 
zinit  =  0; 

magvar  =  -24;    %  Set  initial  magnetic  compass  bias 

gyrovar  -  -1\    %  Set  gyro  bias 

%  Initial  conditions 

x_()ACC  =  [();();()]; 

x_()AR  =  [();0;()]; 

x_()V  =  [();().5;()];%  Velocities  (x,y,z) 

x_()XY  =  [()-xinit;()-yinit];  %  Position  (x,y) 

x_()Z  =  zinit-();7r'  Position  (z) 

x_()R  =  0*DTOR;%Roll 

x_()P  =  ()*DTOR;%  Pitch 

xJ)H  =  (3{)()+gyrovar)*DTOR;%  Heading 

x_() 

[x_()ACC;x_()V;x_()XY;x_()Z;xJ)AR;x_{)R;x_()P;x_()H;gyrovar*DTOR;magvar*DTOR] 


%  Initial  En^or  covariance  matrix 

P_()  =  zeros(17);  %  Initial  P  matrix  for  states 

PJ)(1:3,1:3)  =  (().l*eye(3)).^2; 

P_()(4:6,4:6)  =  (.()2*eye(3)).^2;  %  Initial  uncertainty  in  velocity  (m/s) 

P_()(7:8,7:8)  =  (3*eye(2)).^2;  %  Initial  uncertainly  in  position  (m) 

P_()(9,9)  =  0.3^2;         %  Initial  uncertainty  in  depth  (m) 

P_()(l():12,l():12)  =  (().5*DTOR*eye(3)).^2; 

P_()(I3:14,13:14)  =  (l*DTOR*eye(2))^2;  %  Initial  uncertainty  in  roll  and  pitch 

P_()(15,15)  =  (2*DT()Rr2;%  Initial  uncertainty  in  heading 

PJ)(16,16)  =  (2*DT()Rr2;%  Set  initial  gyro  bias  variance 

P_{)(17,17)  =  (2*DTOR)'^2;%  Set  initial  magnetic  compass  bias  variance 
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w  =  zeros(6,l);%  random  forcing  function 

g  =  -9.8;%  gravity  vector 

%  Set  IMU  biases  to  be  subtracted  from  raw  data  (units  of  g  for  ace,  degrees/s  for 

%  angular  rates) 

xaccbias  =  0; 
yaccbias'=  0; 
zaccbias  =  0; 
rrbias  =  0; 
prbias  =  0; 
yrbias  =  0; 
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KFRun 


%dt  =  1/24/3600/2;  %  Gives  desired  kalman  filter  update  time  step  of  0.5  seconds 

tO  =  tinit;     %  Set  initial  time  for  filter  for  calculating  time  steps 

t  =  tO;     - 

tfix  =  tO;        %  Time  of  last  fix 

kk  =  1;  %  Index  for  state  estimate  file 

nn  =  0; 

mm  =  0; 

qq=l; 

zl  =  zeros(3,l); 

z2  =  zeros(3,l); 

Posmat  =  zeros(4,2); 

fixcount  =  0; 

SWAcheck  =  ();%  Set  Counters  to  count  lines  of  each  type  of  data  in  filter  step 

PAScheck  =  0; 

PVScheck  =  0; 

LBLcheck  =  0; 

FixedSim2=zeros(720(),18);  %  Matrix  to  hold  processed  raw  data 

Templn=[]; 

EstSim2  =  []; 

rejfix  =  0;  %  Counter  to  keep  track  of  rejected  fixes 

Xp  =  x_();    %  Set  initial  condition  for  estimated  states 

Pp  =  P_0;    %  Set  initial  condition  for  error  covariance  matrix 

while  nn  <  7200;    %  Setup  loop  to  read  all  data  from  file 

%  The  following  loop  ensures  both  SWA  and  PAS  data  are  available  to  the  filter 

while  SWAcheck  ==  0  I  PAScheck  ==  0 

mm  =  mm+1; 

nn  =  nn+1; 

Templn(mm,:)  =  SlM(nn,:);  %  SIM  is  data  file 

Templn(mm,l)  =  Templn(mm,l)-tinit; 

[M,N]  =  size(TempIn); 

t  =  TempIn(M,l); 

%  This  loop  reads  the  next  dt  worth  of  data  from  the  file.  For  real-time 

%  processing,  use  same  principle  to  read  dt  worth  of  data  into  Templn. 

while  Templn(mm,  1  )-t()  <=  dt;  %  Check  to  see  if  have  dt  worth  of  data 

nn  =  nn+1; 

mm  =  mm+1; 

Templn(mm,:)  =  SIM(nn,:); 

Templn(mm,l)  =  Templn(mm,l)-tinit; 

[M,N]  =  size(Templn); 

t  =  TempIn(M,l); 
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end 

%  This  loop  checks  if  both  SWA  and  PAS  data  is  present.  If  the  data  does  not 

%  contain  at  least  one  sample  of  both  SWA  and  PAS  data,  another  dt  worth  will 

%  be  added  prior  to  proceeding. 

for  i  =  qq:M 

if  Templn(i,2)  <1()631  I  Templn(i,2)  >  10633 

SWAcheck  =  SWAcheck+1; 

else 

Templn(i,2:7)  =  zeros(l,6);  %  set  bogus  data  to  zero 

end 

if  Templn(i,13)  <10631  I  Templn(i,13)  >  10633 

PAScheck  =  PAScheck+1; 

Templn(i,13)  =  zinit-TempIn(i,13); 

TempIn(i,14:15)  =  TempIn(i,14:15).*DTOR; 

if(360-TempIn(l,16))<75ITempIn(l,16)<75 

ifTempIn(i,16)>27() 

Templn(i,  1 6)=TempIn(i,  1 6)-36(); 

else 

end 

else 

end 

Templn(i,16)  =  TempIn(i,16)*DTOR; 

if  (36()-TempIn(  1 ,  17))<75  I  Templn(  1 ,  17)<75 

ifTempIn(i,17)>27() 

TempIn(i,17)=TempIn(i,17)-360; 

else 

end 

else 

end 

Templn(i,17)  =  TempIn(i,17)*DT0R; 

else 

Templn(i,13;17)  =  zeros(l,5);  %  set  bogus  data  to  zero 

end 

if  Templn(i,8)  <1{)631  I  Templn(i,8)  >  10633 

PVScheck  =  PVScheck+1; 
else 

Templn(i,8:l())  =  zeros(l,3);  %  set  bogus  data  to  zero 
Templn(i,18)=p; 
end 
if  Templn(i,l  1)  <1()631  I  Templn(i,l  1)  >  10633 

LBLcheck  =  LBLcheck+1; 
Templn(i,l  1)  =  Templn(i,l  l)-xinit; 
Templn(i,12)  =  Templn(i,12)-yinit; 
else 
Templn(i,l  1:12)  =  zeros(l,2);  %  set  bogus  data  to  zero 
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end 

end 

qq  =  M+1; 

end 

FixedSim2((nn+l-M):nn+l-l,:)=TempIn; 

%  Now  we  will  provide  the  estimates  of  x^(t+llt)  and  P(t+llt).  These  are  done 

%  now  rather  as  the  last  step  in  the  tllter  propagation  due  to  the  variable 

%  time  step.  Now  that  we  know  the  time  step,  we  can  more  accurately  project 

%  the  step  estimates  into  the  future. 

%DT  =  (t-t())*24*36()();    %  This  is  the  length  of  the  current  filter  time  step. 

[Xm,Pm]  =  fullkfpred(A,G,w,Q,Xp,Pp,DT); 

%  We  now  have  enough  data  to  run  the  filter.  For  data  with  more  than  one  time 

%  sample's  worth  present,  the  data  for  the  whole  time  step  is  averaged. 

%  Also,  any  suspected  bias  is  removed.  The  inputs  are  converted  into  the  proper 

%  units  for  filter  use.  Also,  the  data  is  checked  to  see  if  LBL  and/or  doppler 

%  data  is  available,  which  will  deteimine  which  version  of  the  filter  to  use. 

%  Therefore,  the  input  to  the  kalman  filter  will  have  only  one  line  of 

%  averaged  data,  which  will  always  include  SWA  and  PAS  data,  and  may  also 

%  have  LBL  and/or  doppler  data. 

zl(l)  =  (((sum(TempIn(:,2))/SWAcheck))-xaccbias)*(-g)+((sin(Xp(13)))*g); 

zl(2)  =  (((sum(TempIn(:,3))/SWAcheck))-yaccbias)*(-g)-((sin(Xp(14)))*g); 

zl(3)  =  (((sum(TempIn(:,4))/SWAcheck))-zaccbias)*(-g)- 

((cos(Xp(13))*cos(Xp(14)))*g); 

z2(l)  =  (((sum(TempIn(:,5))/SWAcheck))-iTbias)*DTOR; 

z2(2)  =  (((sum(TempIn(:,6))/SWAcheck))-prbias)*DTOR; 

z2(3)  =  (((sum(TempIn(:,7))/SWAcheck))-yrbias)*DTOR; 

%zl=zeros(3,l);    %  Used  only  if  not  using  actual  accelerometer  measurements 

%z2=zeros(3,l); 

%  One  other  requirement  is  to  check  for  position  fiyers.  If  a  position  update 

%  is  bogus,  it  will  be  ignored  and  the  filter  run  without  it.  A  position  update 

%  will  be  compared  with  the  current  filter  estimate  of  position.  If  they  differ 

%  by  more  than  five  times  the  std  deviation  of  the  position  uncertainty,  the 

%  position  update  will  be  ignored. 

if  LBLcheck  ~=  0 

tcheck  =  (t-tfix); 

Pcheckx  =  sum(TempIn(:,l  l))/LBLcheck; 

Pchecky  =  sum(TempIn(:,12))/LBLcheck; 

errest  =  sqrt(((Pcheckx-Xp(7)r2)+(Pchecky-Xp(8))^2); 

if  errest  >  (l()*RPos(l,I))+(().()5*tcheck) 

rejt"ix  =  rejfix+l; 

LBLcheck  =  0;  ^ 

else 

tfix  =  t; 
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end 
end 

%  The  first  case  is  when  all  data  is  availahle.  True  heading  will  only  be 
%  available  after  five  fixes  have  been  obtained. 

%  Angular  measurements  have  already  been  converted  to  radians  using  DTOR. 
if  LBLcheck  ~=  0  &  PVScheck  ~=  0 
fixcount=  tlxcount+1; 
Z  =  zeros(17,l); 
Z(I:3)  =  zl; 

Z(4)  =  sum(TempIn(:,8))/PVScheck; 
Z(5)  =  sum(TempIn(:,9))/PVScheck; 
Z(6)  =  sum(TempIn(;,l()))/PVScheck; 
xLBL  =  sum(TempIn(:,I  l))/LBLcheck; 
Z(7)=  xLBL; 

yLBL  =  sum(TempIn(:,i2))/LBLcheck; 
Z(8)=  yLBL; 

Z(9)  =  sum(TempIn(:,13))/PAScheck; 
Z(10:12)  =  z2; 

Z(13)  =  sum(TempIn(:,14))/PAScheck; 
Z(14)  =  sum(TempIn(:,l5))/PAScheck; 
ghdg  =  sum(TempIn(:,16))/PAScheck; 
if  ghdg<() 

ghdg  =  ghdg+(2*pi); 
else 
end 

Z(15)  =  ghdg; 

mhdg  =  sum(TempIn(:,17))/PAScheck; 
if  mhdg<() 

mhdg  =  mhdg+(2*pi); 
else 
end 

Z(16)  =  mhdg; 
R  =  zeros(17); 
R(1;3,1:3)  =  RACC; 

R(4:6,4:6)  =  eye(3).*((sum(TempIn(:,18))/PVScheck)^2); 
R(7:8,7:8)  =  RPos; 
R(9,9)  =  RDepth; 
R(1():12,1():12)  =  RAR; 
R(13;14,13:14)  =  RPR; 

R(15,15)  =  RHG; 
R(16,16)  =  RHM; 
C  =  CF; 

%  Update  the  matnx  containing  the  last  four  position  fixes 
Posmat(l,:)  =  Posmat(2,:); 
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Posmat(2,:)  =  Posmat(3,:); 

Posmat(3,:)  =  Posmat(4,:); 

Posmat(4,:)  =  [xLBL,yLBL]; 

if  fixcount<4 

Z  =  Z(1:16); 

R  =  R(1:16,1:16); 

C  =  CF(1:16,:); 

else 

%  Now,  calculate  true  heading  using  a  least-squares  fit  to  the  last  four  fixes 

L  =  [l,Posmat(l,l);l,Posmat(2,l);l,Posmat(3,l);l,xLBL]; 

B  =  [Posmat(l,2);Posmat(2,2);Posmat(3,2);yLBL]; 

bfit=  inv(L'*L)*L'*B; 

if  bfit(2)>=0  &  yLBL  >  Posmat(l,2) 

thdg  =  (pi/2)-atan(brit(2)); 

elseif  bfit(2)<()  &  yLBL  <  Posmat(l,2) 

thdg  =  (pi/2)-atan(bfit(2)); 

else 

thdg  =  (3*pi/2)-atan(bfit(2)); 

end 

Z(17)  =  thdg; 

%  Calculate  variance  of  this  heading  measurement 

[HV]  =  hvar(Posmat,thdg); 

R(17,17)  =  [HV]; 

end 

%  The  second  case  is  when  SWA,  PAS,  and  LBL  data  is  available,  but  no  doppler. 

elseif  LBLcheck  ~=  0  &  PVScheck  ==  0 

fixcount  =  fixcount+1; 

Z  =  zeros(14,l); 

Z(l:3)  =  zl; 

xLBL  =  sum(TempIn(:,ll))/LBLcheck; 

Z(4)=  xLBL; 

yLBL  =  sum(TempIn(:,12))/LBLcheck; 

Z(5)=  yLBL; 

Z(6)  =  sum(TempIn(:,13))/PAScheck; 

Z(7:9)  =  z2; 

Z(l())  =  sum(TempIn(:,14))/PAScheck; 

Z(il)  =  sum(TempIn(:,15))/PAScheck; 

ghdg  =  sum(TempIn(:,16))/PAScheck; 

if  ghdg<() 

ghdg  =  ghdg+(2*pi); 

else  '' 

end 

Z(12)  =  ghdg; 


99 


mhdg  =  sum(TempIn(;,l7))/PAScheck;; 
if  mhdg<() 

mhdg  =  mhdg+(2*pi); 
else 
end 

Z(13)  =  mhdg; 
R  =  zeros(14,14); 
R(1:3,1:3)  =  RACC; 
R(4:5,4:5)  =  RPos; 
R(6,6)  =  RDepth; 
R(7:9,7:9)  =  RAR; 
R(10:11,10:11)  =  RPR; 
R(12,12)  =  RHG; 
R(13,13)  =  RHM; 

C  =  CNV;  %  Update  the  matrix  containing  the  last  four  position  tlxes 
Posmat(l,:)  =  Posmat(2,:); 
Posmat(2,:)  =  Posmat(3,:); 
Posmat(3,:)  =  Posmat(4,:); 
Posmat(4,:)  =  [xLBL,yLBL]; 
if  fixcount<4 
Z  =  Z(1:13); 
R  =  R(1:13,1:13); 
C  =  CNV(1:13,:); 
else 

%  Calculate  true  heading 

L  =  [l,Posmat(l,l);l,Posmat(2,l);l,Posmat(3,l);l,xLBL]; 
B  =  [Posmat(l,2);Posmat(2,2);Posmat.(3,2);yLBL]; 
bfit=  inv(L'*L)*L'*B; 
if  bfit(2)>=()  &  yLBL  >  Posmat(l,2) 
thdg  =  (pi/2)-atan(bfit(2)); 
elseif  bfit(2)<()  &  yLBL  <  Posmat(l,2) 
thdg  =  (pi/2)-atan(bfit(2)); 
else 

thdg  =  (3*pi/2)-atan(bfit(2)); 
end 

Z(14)  =  thdg; 

%  Calculate  vanance  of  this  heading  measurement 
[HV]  =  hvar(Posmat,thdg); 
R(14,14)  =  [HVJ; 
end 

%  The  third  case  is  when  SWA,  PAS,  and  doppler  data  is  available,  but  no  LBL. 

elseif  LBLcheck  ==  0  &  PVScheck  ~=  0 

Z  =  zeros(14,l); 
Z(l:3)  =  zl; 
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Z(4)  =  sum(TempIn(:,8))/PVSchcck; 
Z(5)  =  sum(TempIn(:,9))/PVScheck; 
Z(6)  =  sum(TempIn(:,l()))/PVScheck; 
Z(7)  =  sum(TempIn(:,13))/PAScheck; 
Z(8:lO)  =  z2; 

Z(ll)  =  sum(TempIn(:,14))/PAScheck; 
Z(12)  =  sum(TempIn(:,15))/PAScheck; 
ghdg  =  sum(TempIn(:,16))/PAScheck; 
if  ghdg<() 

ghdg  =  ghdg+(2*pi); 
else 
end 

Z(13)  =  ghdg; 

mhdg  =  sum(TempIn(:,17))/PAScheck; 
if  mhdg<0 

mhdg  =  mhdg+(2*pi); 
else 
end 

Z(14)  =  mhdg; 
R  =  zeros(14); 
R(1:3,1:3)  =  RACC; 

R(4:6,4:6)  =  eye(3).*((sum(TempIn(:,18))/PVScheck)'^2); 
R(7,7)  =  RDepih; 
R(8:1(),8:1())  =  RAR; 
R(11:12,11:12)  =  RPR; 
R(13,i3)  =  RHG; 
R(14,14)  =  RHM; 
C  =  CNP; 

%  The  last  case  is  when  only  SWA  and  PAS  data  is  available. 

else 

Z  =  zeros(ll,l); 

Z(l:3)  =  zl, 

Z(4)  =  sum(TempIn(:,13))/PAScheck; 

Z(5:7)  =  z2; 

Z(8)  =  sum(TempIn(:,14))/PAScheck; 

Z(9)  =  sum(TempIn(:,15))/PAScheck; 

ghdg  =  sum(TempIn(:,16))/PAScheck; 

if  ghdg<() 

ghdg  =  ghdg+(2*pi); 

else 

end 

Z(l())  =  ghdg; 

mhdg  =  sum(TempIn(:,I7))/PAScheck; 
if  mhdg<() 
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mhdg  =  mhdg+(2*pi); 

else 

end 

Z(ll)  =  mhdg; 

R  =  zeros(ll); 

R(1:3,1:3)  =  RACC; 

R(4,4)  =  RDepth; 

R(5:7,5:7)  =  RAR; 

R(8:9,8:9)  =  RPR; 

R(1(),1())  =  RHG; 

R(11,11)  =  RHM; 

C  =  CNPV; 

end 

%  Now  the  filter  can  finally  be  run  using  KFcalc. 

[Xp,Pp]=fullkfcalc(C,R,Xm,Pm,Z); 

EstSim2(kk,l:18)  =  [Xp',tl;  %  Store  state  estimates  with  time  stamp  nn 
SWAcheck  =  0; 
PAScheck  =  0; 
LB  Lc  heck  =  0 
PVScheck  =  0 
mm  =  0; 
to  =  t; 
qq=l; 
Tempin  =  []; 
kk  =  kk+l; 
pp=l; 
end 
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KFpred 


%  The  function  kfpred  provides  the  estimates  of  x'^Ct+lIt)  and  P(t+llt). 

%  It  must  be  done  out  of  the  normal  order  for  a  Kalman  filter  since  the  time 

%  step  is  unknown  until  the  next  block  of  data  is  processed. 

%  The  format  is;  [Xm,Pml=kfpred(A,G,U,Q,xcurr,P,t) 

%  t  is  the  time  stamp  for  the  current  update  step. 

function[Xm,Pm]=:kfpred(A,G,U,Q,xcurr,P,t) 

headg  =  xcurr(15); 

pitch  =  xcurr(14); 

roll  =  xcurr(13); 

dheadg  =  (2*pi  -  headg);  %  Change  axes 

pcos  =  cos(pitch); 

psin  =  sin(pitch); 

rcos  =  cos(roll); 

rsin  =  sin(roll); 

hcos  =  cos(dheadg); 

hsin  =  sin(dheadg); 

A(7,4)  =  rcos*hcos; 

A(7,5)  =  psin*rsin*hcos  -  pcos*hsin; 

A(7,6)  =  pcos*rsin*hcos  +  psin*hsin; 

A(8,4)  =  hsin*rcos; 

A(8,5)  =  psin*rsin*hsin  +  pcos*hcos; 

A(8,6)  =  pcos*rsin*hsin  -  psin*hcos; 

A(9,4)  =  -rsin;  A(9,5)  =  psin*rcos; 

A(9,6)  =  pcos*rcos; 

[Phi,Gamma]  =  c2d(A,G,t); 

Xm  =  Phi*xcurr  +  Gamma*U;  %  Xm  =  x'^lt+llt) 

%Pm  =  l.{)168*Phi*P*Phi'  +  Gamma*Q*Gamma';  %  Use  for  maneuvering  vehicle 

Pm  =  l.()()l*Phi*P*Phi'  +  Gamma*Q*Gamma';    %  Use  when  have  position  data  gaps  or 

no  maneuvering 
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FullKFcalc 


%  The  function  FuUKFCalc  implements  the  kalman  filter  for  full  data  sets 
%  The  format  is:  [Xp,Pp]=fullkfcalc(C,R,Xm,P,Z) 
%  t  is  the  time  stamp  for  the  current  update  step. 
function[Xp,Pp]=fullkfcalc(C,R,Xm,P,Z) 
while  Xm(15)<() 

Xm(15)  =  Xm(15)+(2*pi); 

end 

while  Xm(15)>2*pi 

Xm(15)  =  Xm(15)-(2*pi); 

end 

while  Xm(i6)>pi 

Xm(16)=Xm(16H2*pi); 

end 

while  Xm(16)<(-pi) 

Xm(16)=Xm(16)+(2*pi); 

end 

while  Xm(17)>pi 

Xm(17)=Xm(17)-(2*pi); 

end 

while  Xm(17)<(-pi) 

Xm(17)=Xm(l7)+(2*pi); 

end 

Ze  =  C*Xm;  %  Ze  =  y^(tlt-l) 

I  =  Z-Ze;  rr.I^yltVy'^Ctlt-l) 

V  =  C*P*C'+R;  %  V  =  covariance  matrix  of  innovations 

k  =  p*C'*inv(v); 

Xp  =  Xm  +  k*I;  %  xp  =  x'^(tlt) 

while  Xp(15)<() 

Xp(15)  =  Xp(i5)+(2*pi); 

end 

whileXp(15)>2*pi 

Xp(15)  =  Xp(15)-(2*pi); 

end 

while  Xp(16)>pi 

Xp(16)=Xp(16)-(2*pi); 

end 

while  Xp(16)<(-pi) 

Xp(16)=Xp(16)+(2*pi); 

end 

while  Xp(17)>pi 

Xp(17)=Xp(17H2*pi); 
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end 

while  Xp(17)<(-pi) 

Xp(17)=Xp(17)+(2*pi); 

end 

temp  =  eye(17)-(k*C); 

Pp  =  temp*P*temp'+k*R*k'  ;  %  p  =  P(tlt) 
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HVar 


%  The  function  hvar  provides  the  variance  of  the  ime  heading 
%  measurement. 

%  The  format  is  [HV]  =  hvar(Posmat,thdg) 
function[HV]  =  hvar(Posmat,thdg) 
tempvar  =  0; 
Htemp  =  zeros(3,l); 
fori=  1:3; 

LL  =  [  1  ,Posmat(i,  1 );  1  ,Posmat(i+ 1,1)]; 
BB  =  [Posmat(i,2);Posmat(i+l,2)]; 
bbfit=  inv(LL'*LL)*LL'*BB; 
if  bbfit(2)>=()  &  BB(2)>BB(1) 
temphdg  =  (pi/2)-atan(bbfit(2)); 
elseif  bbrit(2)<()  &  BB(2)<BB(1) 
temphdg  =  (pi/2)-atan(bbfit(2)); 
else  temphdg  =  (3*pi/2)-atan(bbnt(2)); 
end 

Htemp(i)  =  temphdg; 
end 

fori=  1:3; 
if  Htemp(i)-thdg>pi 
HVar(i)  =  ((Htemp(i)-(2*pi))-thdg)'^2; 
elseif  thdg-Htemp(i)>pi 
HVar(i)  =  ((thdg-(2*pi))-Htemp(i)r2; 
else 

HVar(i)  =  (Htemp(i)-thdg)^2; 
end 
end 
HV  =  (sum(HVar))/3; 
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